From 5aa46af99570158e7de7eb1bb7e69b8745102c0f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 09:53:28 -0500 Subject: [PATCH 001/152] Rename header files to have .hpp extension. Signed-off-by: Chris Lalancette --- .../{async_worker.h => async_worker.hpp} | 4 ++-- .../ublox_gps/{callback.h => callback.hpp} | 2 +- ublox_gps/include/ublox_gps/{gps.h => gps.hpp} | 6 +++--- .../include/ublox_gps/{node.h => node.hpp} | 18 +++++++++--------- .../{raw_data_pa.h => raw_data_pa.hpp} | 0 .../include/ublox_gps/{utils.h => utils.hpp} | 0 .../include/ublox_gps/{worker.h => worker.hpp} | 0 ublox_gps/src/gps.cpp | 2 +- ublox_gps/src/logger_node_pa.cpp | 2 +- ublox_gps/src/node.cpp | 2 +- ublox_gps/src/raw_data_pa.cpp | 2 +- .../{ublox_msgs.h => ublox_msgs.hpp} | 10 +++++----- .../{ublox_msgs.h => ublox_msgs.hpp} | 6 +++--- ublox_msgs/src/ublox_msgs.cpp | 2 +- .../include/ublox/{checksum.h => checksum.hpp} | 6 +++--- .../{serialization.h => serialization.hpp} | 12 ++++++------ ...rialization_ros.h => serialization_ros.hpp} | 10 +++++----- 17 files changed, 42 insertions(+), 42 deletions(-) rename ublox_gps/include/ublox_gps/{async_worker.h => async_worker.hpp} (99%) rename ublox_gps/include/ublox_gps/{callback.h => callback.hpp} (99%) rename ublox_gps/include/ublox_gps/{gps.h => gps.hpp} (99%) rename ublox_gps/include/ublox_gps/{node.h => node.hpp} (99%) rename ublox_gps/include/ublox_gps/{raw_data_pa.h => raw_data_pa.hpp} (100%) rename ublox_gps/include/ublox_gps/{utils.h => utils.hpp} (100%) rename ublox_gps/include/ublox_gps/{worker.h => worker.hpp} (100%) rename ublox_msgs/include/ublox/serialization/{ublox_msgs.h => ublox_msgs.hpp} (99%) rename ublox_msgs/include/ublox_msgs/{ublox_msgs.h => ublox_msgs.hpp} (99%) rename ublox_serialization/include/ublox/{checksum.h => checksum.hpp} (96%) rename ublox_serialization/include/ublox/{serialization.h => serialization.hpp} (98%) rename ublox_serialization/include/ublox/{serialization_ros.h => serialization_ros.hpp} (94%) diff --git a/ublox_gps/include/ublox_gps/async_worker.h b/ublox_gps/include/ublox_gps/async_worker.hpp similarity index 99% rename from ublox_gps/include/ublox_gps/async_worker.h rename to ublox_gps/include/ublox_gps/async_worker.hpp index c41e74d1..b4cf086b 100644 --- a/ublox_gps/include/ublox_gps/async_worker.h +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -29,7 +29,7 @@ #ifndef UBLOX_GPS_ASYNC_WORKER_H #define UBLOX_GPS_ASYNC_WORKER_H -#include +#include #include #include @@ -38,7 +38,7 @@ #include -#include "worker.h" +#include "worker.hpp" namespace ublox_gps { diff --git a/ublox_gps/include/ublox_gps/callback.h b/ublox_gps/include/ublox_gps/callback.hpp similarity index 99% rename from ublox_gps/include/ublox_gps/callback.h rename to ublox_gps/include/ublox_gps/callback.hpp index bbf51f2d..43198a1c 100644 --- a/ublox_gps/include/ublox_gps/callback.h +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -30,7 +30,7 @@ #define UBLOX_GPS_CALLBACK_H #include -#include +#include #include #include #include diff --git a/ublox_gps/include/ublox_gps/gps.h b/ublox_gps/include/ublox_gps/gps.hpp similarity index 99% rename from ublox_gps/include/ublox_gps/gps.h rename to ublox_gps/include/ublox_gps/gps.hpp index 468aae28..8bc7ced9 100644 --- a/ublox_gps/include/ublox_gps/gps.h +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -42,10 +42,10 @@ // ROS #include // Other u-blox packages -#include +#include // u-blox gps -#include -#include +#include +#include /** * @namespace ublox_gps diff --git a/ublox_gps/include/ublox_gps/node.h b/ublox_gps/include/ublox_gps/node.hpp similarity index 99% rename from ublox_gps/include/ublox_gps/node.h rename to ublox_gps/include/ublox_gps/node.hpp index 0f33aa41..27bed2fb 100644 --- a/ublox_gps/include/ublox_gps/node.h +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -27,8 +27,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_NODE_H -#define UBLOX_GPS_NODE_H +#ifndef UBLOX_GPS_NODE_HPP +#define UBLOX_GPS_NODE_HPP // STL #include @@ -51,11 +51,11 @@ #include #include // Other U-Blox package includes -#include +#include // Ublox GPS includes -#include -#include -#include +#include +#include +#include // This file declares the ComponentInterface which acts as a high level // interface for u-blox firmware, product categories, etc. It contains methods @@ -1087,7 +1087,7 @@ class AdrUdrProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics() { ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", - "unimplemented. See AdrUdrProduct class in node.h & node.cpp."); + "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); } protected: @@ -1113,7 +1113,7 @@ class FtsProduct: public virtual ComponentInterface { */ void getRosParams() { ROS_WARN("Functionality specific to u-blox FTS devices is %s", - "unimplemented. See FtsProduct class in node.h & node.cpp."); + "unimplemented. See FtsProduct class in node.hpp & node.cpp."); } /** @@ -1376,4 +1376,4 @@ class TimProduct: public virtual ComponentInterface { } -#endif +#endif // UBLOX_GPS_NODE_HPP diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.h b/ublox_gps/include/ublox_gps/raw_data_pa.hpp similarity index 100% rename from ublox_gps/include/ublox_gps/raw_data_pa.h rename to ublox_gps/include/ublox_gps/raw_data_pa.hpp diff --git a/ublox_gps/include/ublox_gps/utils.h b/ublox_gps/include/ublox_gps/utils.hpp similarity index 100% rename from ublox_gps/include/ublox_gps/utils.h rename to ublox_gps/include/ublox_gps/utils.hpp diff --git a/ublox_gps/include/ublox_gps/worker.h b/ublox_gps/include/ublox_gps/worker.hpp similarity index 100% rename from ublox_gps/include/ublox_gps/worker.h rename to ublox_gps/include/ublox_gps/worker.hpp diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 8073d957..76ccd5ef 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -27,7 +27,7 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include +#include #include namespace ublox_gps { diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 5cb4fb91..9beab0ce 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -35,7 +35,7 @@ // ROS includes #include // Ublox GPS includes -#include +#include using namespace ublox_node; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d7210c4b..7e919a58 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -27,7 +27,7 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include "ublox_gps/node.h" +#include "ublox_gps/node.hpp" #include #include #include diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 872fca9a..1d440977 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -32,7 +32,7 @@ // the data as ros messages. This is used by our group to also evaluate the // measured data with the rtklib. -#include "ublox_gps/raw_data_pa.h" +#include "ublox_gps/raw_data_pa.hpp" #include #include #include diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.h b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp similarity index 99% rename from ublox_msgs/include/ublox/serialization/ublox_msgs.h rename to ublox_msgs/include/ublox/serialization/ublox_msgs.hpp index fc7b2c41..b8048f9a 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.h +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp @@ -27,12 +27,12 @@ //============================================================================== -#ifndef UBLOX_SERIALIZATION_UBLOX_MSGS_H -#define UBLOX_SERIALIZATION_UBLOX_MSGS_H +#ifndef UBLOX_SERIALIZATION_UBLOX_MSGS_HPP +#define UBLOX_SERIALIZATION_UBLOX_MSGS_HPP #include -#include -#include +#include +#include /// /// This file declares custom serializers for u-blox messages with dynamic @@ -880,4 +880,4 @@ struct Serializer > { } // namespace ublox -#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_H +#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_HPP diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.h b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp similarity index 99% rename from ublox_msgs/include/ublox_msgs/ublox_msgs.h rename to ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index 09c2e80e..990007d3 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.h +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -26,8 +26,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_MSGS_H -#define UBLOX_MSGS_H +#ifndef UBLOX_MSGS_HPP +#define UBLOX_MSGS_HPP #include #include @@ -256,4 +256,4 @@ namespace Message { } //!< namespace ublox_msgs -#endif //!< UBLOX_MSGS_H +#endif //!< UBLOX_MSGS_HPP diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 25511954..f94bb70a 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -26,7 +26,7 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include +#include template std::vector > ublox::Message::keys_; diff --git a/ublox_serialization/include/ublox/checksum.h b/ublox_serialization/include/ublox/checksum.hpp similarity index 96% rename from ublox_serialization/include/ublox/checksum.h rename to ublox_serialization/include/ublox/checksum.hpp index 38041390..8c9d3adb 100644 --- a/ublox_serialization/include/ublox/checksum.h +++ b/ublox_serialization/include/ublox/checksum.hpp @@ -26,8 +26,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_MSGS_CHECKSUM_H -#define UBLOX_MSGS_CHECKSUM_H +#ifndef UBLOX_MSGS_CHECKSUM_HPP +#define UBLOX_MSGS_CHECKSUM_HPP #include @@ -69,4 +69,4 @@ static inline uint16_t calculateChecksum(const uint8_t *data, } // namespace ublox -#endif // UBLOX_MSGS_CHECKSUM_H +#endif // UBLOX_MSGS_CHECKSUM_HPP diff --git a/ublox_serialization/include/ublox/serialization.h b/ublox_serialization/include/ublox/serialization.hpp similarity index 98% rename from ublox_serialization/include/ublox/serialization.h rename to ublox_serialization/include/ublox/serialization.hpp index 5e5cfa72..e0984c69 100644 --- a/ublox_serialization/include/ublox/serialization.h +++ b/ublox_serialization/include/ublox/serialization.hpp @@ -26,8 +26,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_SERIALIZATION_H -#define UBLOX_SERIALIZATION_H +#ifndef UBLOX_SERIALIZATION_HPP +#define UBLOX_SERIALIZATION_HPP #include #include @@ -35,7 +35,7 @@ #include #include -#include "checksum.h" +#include "checksum.hpp" /// /// This file defines the Serializer template class which encodes and decodes @@ -437,7 +437,7 @@ class Writer { } } \ -// use implementation of class Serializer in "serialization_ros.h" -#include "serialization_ros.h" +// use implementation of class Serializer in "serialization_ros.hpp" +#include "serialization_ros.hpp" -#endif // UBLOX_SERIALIZATION_H +#endif // UBLOX_SERIALIZATION_HPP diff --git a/ublox_serialization/include/ublox/serialization_ros.h b/ublox_serialization/include/ublox/serialization_ros.hpp similarity index 94% rename from ublox_serialization/include/ublox/serialization_ros.h rename to ublox_serialization/include/ublox/serialization_ros.hpp index 9236ff46..6b32f65b 100644 --- a/ublox_serialization/include/ublox/serialization_ros.h +++ b/ublox_serialization/include/ublox/serialization_ros.hpp @@ -26,11 +26,11 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_SERIALIZATION_ROS_H -#define UBLOX_SERIALIZATION_ROS_H +#ifndef UBLOX_SERIALIZATION_ROS_HPP +#define UBLOX_SERIALIZATION_ROS_HPP -#include "serialization.h" -#include "checksum.h" +#include "serialization.hpp" +#include "checksum.hpp" #include @@ -58,4 +58,4 @@ void Serializer::write(uint8_t *data, uint32_t size, } // namespace ublox -#endif // UBLOX_SERIALIZATION_ROS_H +#endif // UBLOX_SERIALIZATION_ROS_HPP From a08939dfe515591e2f30704f3fd0241494cbbe0e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 10:04:36 -0500 Subject: [PATCH 002/152] Remove trailing whitespace in all files. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 28 +-- ublox_gps/include/ublox_gps/gps.hpp | 4 +- ublox_gps/include/ublox_gps/mkgmtime.h | 6 +- ublox_gps/include/ublox_gps/node.hpp | 14 +- ublox_gps/include/ublox_gps/utils.hpp | 10 +- ublox_gps/include/ublox_gps/worker.hpp | 8 +- ublox_gps/src/gps.cpp | 4 +- ublox_gps/src/logger_node_pa.cpp | 2 +- ublox_gps/src/mkgmtime.c | 8 +- ublox_gps/src/node.cpp | 58 +++--- .../ublox/serialization/ublox_msgs.hpp | 174 +++++++++--------- ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 50 ++--- ublox_msgs/src/ublox_msgs.cpp | 150 +++++++-------- .../include/ublox/checksum.hpp | 20 +- .../include/ublox/serialization.hpp | 116 ++++++------ .../include/ublox/serialization_ros.hpp | 10 +- 16 files changed, 331 insertions(+), 331 deletions(-) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 43198a1c..87eb6f89 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -69,12 +69,12 @@ class CallbackHandler_ : public CallbackHandler { public: typedef boost::function Callback; //!< A callback function - /** + /** * @brief Initialize the Callback Handler with a callback function * @param func a callback function for the message, defaults to none */ CallbackHandler_(const Callback& func = Callback()) : func_(func) {} - + /** * @brief Get the last received message. */ @@ -88,8 +88,8 @@ class CallbackHandler_ : public CallbackHandler { boost::mutex::scoped_lock lock(mutex_); try { if (!reader.read(message_)) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); @@ -97,8 +97,8 @@ class CallbackHandler_ : public CallbackHandler { return; } } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + ROS_DEBUG_COND(debug >= 2, + "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), reader.length()); @@ -109,7 +109,7 @@ class CallbackHandler_ : public CallbackHandler { if (func_) func_(message_); condition_.notify_all(); } - + private: Callback func_; //!< the callback function to handle the message T message_; //!< The last received message @@ -135,7 +135,7 @@ class CallbackHandlers { } /** - * @brief Add a callback handler for the given message type and ID. This is + * @brief Add a callback handler for the given message type and ID. This is * used for messages in which have the same structure (and therefore msg file) * and same class ID but different message IDs. (e.g. INF, ACK) * @param callback the callback handler for the message @@ -144,7 +144,7 @@ class CallbackHandlers { */ template void insert( - typename CallbackHandler_::Callback callback, + typename CallbackHandler_::Callback callback, unsigned int message_id) { boost::mutex::scoped_lock lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); @@ -188,7 +188,7 @@ class CallbackHandlers { message = handler->get(); result = true; } - + // Remove the callback handler callback_mutex_.lock(); callbacks_.erase(callback); @@ -212,7 +212,7 @@ class CallbackHandlers { for (ublox::Reader::iterator it = reader.pos(); it != reader.pos() + reader.length() + 8; ++it) oss << boost::format("%02x") % static_cast(*it) << " "; - ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, + ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, oss.str().c_str()); } diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 8bc7ced9..268e01b8 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -299,7 +299,7 @@ class Gps { bool setUseAdr(bool enable); /** - * @brief Configure the U-Blox to UTC time + * @brief Configure the U-Blox to UTC time * @return true on ACK, false on other conditions. * * @note This is part of the expert settings. It is recommended you check @@ -316,7 +316,7 @@ class Gps { * the ublox manual first. */ bool setTimtm2(uint8_t rate); - + /** * @brief Configure the U-Blox send rate of the message & subscribe to the * given message diff --git a/ublox_gps/include/ublox_gps/mkgmtime.h b/ublox_gps/include/ublox_gps/mkgmtime.h index 8537379c..ba8142d8 100644 --- a/ublox_gps/include/ublox_gps/mkgmtime.h +++ b/ublox_gps/include/ublox_gps/mkgmtime.h @@ -1,6 +1,6 @@ /* mkgmtime.h -- make a time_t from a gmtime struct tm $Id: mkgmtime.h,v 1.5 2003/02/13 20:15:41 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 27bed2fb..93d81a03 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -1094,7 +1094,7 @@ class AdrUdrProduct: public virtual ComponentInterface { //! Whether or not to enable dead reckoning bool use_adr_; - + sensor_msgs::Imu imu_; sensor_msgs::TimeReference t_ref_; ublox_msgs::TimTM2 timtm2; @@ -1343,13 +1343,13 @@ class TimProduct: public virtual ComponentInterface { * @brief Get the Time Sync parameters. * @todo Currently unimplemented. */ - void getRosParams(); - + void getRosParams(); + /** * @brief Configure Time Sync settings. * @todo Currently unimplemented. */ - bool configureUblox(); + bool configureUblox(); /** * @brief Subscribe to Time Sync messages. @@ -1364,13 +1364,13 @@ class TimProduct: public virtual ComponentInterface { */ void initializeRosDiagnostics(); - protected: + protected: /** - * @brief + * @brief * @details Publish recieved TimTM2 messages if enabled */ void callbackTimTM2(const ublox_msgs::TimTM2 &m); - + sensor_msgs::TimeReference t_ref_; }; diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index ee649eae..1868031a 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -16,14 +16,14 @@ template long toUtcSeconds(const NavPVT& msg) { // Create TM struct for mkgmtime struct tm time = {0}; - time.tm_year = msg.year - 1900; - time.tm_mon = msg.month - 1; + time.tm_year = msg.year - 1900; + time.tm_mon = msg.month - 1; time.tm_mday = msg.day; - time.tm_hour = msg.hour; - time.tm_min = msg.min; + time.tm_hour = msg.hour; + time.tm_min = msg.min; time.tm_sec = msg.sec; // C++ STL doesn't have a mkgmtime (though other libraries do) - // STL mktime converts date/time to seconds in local time + // STL mktime converts date/time to seconds in local time // A modified version of code external library is used for mkgmtime return mkgmtime(&time); } diff --git a/ublox_gps/include/ublox_gps/worker.hpp b/ublox_gps/include/ublox_gps/worker.hpp index 6a9bef52..661274a3 100644 --- a/ublox_gps/include/ublox_gps/worker.hpp +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -60,7 +60,7 @@ class Worker { * @param size the size of the buffer */ virtual bool send(const unsigned char* data, const unsigned int size) = 0; - + /** * @brief Wait for an incoming message. * @param timeout the maximum time to wait. diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 76ccd5ef..8ac95fc5 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -119,7 +119,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - + if(BOOST_VERSION < 106600) { // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but @@ -576,7 +576,7 @@ bool Gps::setTimtm2(uint8_t rate) { ublox_msgs::CfgMSG msg; msg.msgClass = ublox_msgs::TimTM2::CLASS_ID; msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID; - msg.rate = rate; + msg.rate = rate; return configure(msg); } } // namespace ublox_gps diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 9beab0ce..6c05917e 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -46,7 +46,7 @@ using namespace ublox_node; int main(int argc, char** argv) { ros::init(argc, argv, "ublox_logger"); - + RawDataStreamPa node(true); node.getRosParams(); node.initialize(); diff --git a/ublox_gps/src/mkgmtime.c b/ublox_gps/src/mkgmtime.c index a2feca1a..e80d28ef 100644 --- a/ublox_gps/src/mkgmtime.c +++ b/ublox_gps/src/mkgmtime.c @@ -1,6 +1,6 @@ /* mkgmtime.c - make time corresponding to a GMT timeval struct $Id: mkgmtime.c,v 1.10 2003/10/22 18:50:12 rjs3 Exp $ - + * Copyright (c) 1998-2003 Carnegie Mellon University. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -8,7 +8,7 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in @@ -18,7 +18,7 @@ * 3. The name "Carnegie Mellon University" must not be used to * endorse or promote products derived from this software without * prior written permission. For permission or any other legal - * details, please contact + * details, please contact * Office of Technology Transfer * Carnegie Mellon University * 5000 Forbes Avenue @@ -91,7 +91,7 @@ #define WRONG (-1) #endif /* !defined WRONG */ -static int tmcomp(register const struct tm * const atmp, +static int tmcomp(register const struct tm * const atmp, register const struct tm * const btmp) { register int result; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 7e919a58..9c3b591f 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -220,7 +220,7 @@ void UbloxNode::getRosParams() { // activate/deactivate any config nh->param("config_on_startup", config_on_startup_flag_, true); - // raw data stream logging + // raw data stream logging rawDataStreamPa_.getRosParams(); } @@ -1314,7 +1314,7 @@ void AdrUdrProduct::subscribe() { // also publish sensor_msgs::Imu gps.subscribe(boost::bind( &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); - + // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) @@ -1336,32 +1336,32 @@ void AdrUdrProduct::subscribe() { void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { if (enabled["esf_meas"]) { - static ros::Publisher imu_pub = + static ros::Publisher imu_pub = nh->advertise("imu_meas", kROSQueueSize); static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); - + imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; - + float deg_per_sec = pow(2, -12); float m_per_sec_sq = pow(2, -10); float deg_c = 1e-2; - + std::vector imu_data = m.data; for (int i=0; i < imu_data.size(); i++){ unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits - + if (data_sign == 0) { data_sign = -1; } else { data_sign = 1; } - + //ROS_INFO("data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray - + imu_.orientation_covariance[0] = -1; imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; @@ -1405,31 +1405,31 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; } } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); + //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); } else { ROS_INFO("data_type: %u", data_type); ROS_INFO("data_value: %u", data_value); - } - + } + // create time ref message and put in the data //t_ref_.header.seq = m.risingEdgeCount; //t_ref_.header.stamp = ros::Time::now(); //t_ref_.header.frame_id = frame_id; - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - + //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + //std::ostringstream src; - //src << "TIM" << int(m.ch); + //src << "TIM" << int(m.ch); //t_ref_.source = src.str(); t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + time_ref_pub.publish(t_ref_); imu_pub.publish(imu_); } } - + updater->force_update(); } // @@ -1762,9 +1762,9 @@ void TimProduct::getRosParams() { bool TimProduct::configureUblox() { uint8_t r = 1; // Configure the reciever - if(!gps.setUTCtime()) + if(!gps.setUTCtime()) throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); - + if(!gps.setTimtm2(r)) throw std::runtime_error(std::string("Failed to Configure TIM Product")); @@ -1779,15 +1779,15 @@ void TimProduct::subscribe() { gps.subscribe(boost::bind( &TimProduct::callbackTimTM2, this, _1), kSubscribeRate); - + ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); - + // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) gps.subscribe(boost::bind( publish, _1, "rxmsfrb"), kSubscribeRate); - + // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) @@ -1796,31 +1796,31 @@ void TimProduct::subscribe() { } void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - + if (enabled["tim_tm2"]) { static ros::Publisher publisher = nh->advertise("timtm2", kROSQueueSize); static ros::Publisher time_ref_pub = nh->advertise("interrupt_time", kROSQueueSize); - + // create time ref message and put in the data t_ref_.header.seq = m.risingEdgeCount; t_ref_.header.stamp = ros::Time::now(); t_ref_.header.frame_id = frame_id; - t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - + t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + std::ostringstream src; - src << "TIM" << int(m.ch); + src << "TIM" << int(m.ch); t_ref_.source = src.str(); t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - + publisher.publish(m); time_ref_pub.publish(t_ref_); } - + updater->force_update(); } diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp index b8048f9a..d7b2771d 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -35,7 +35,7 @@ #include /// -/// This file declares custom serializers for u-blox messages with dynamic +/// This file declares custom serializers for u-blox messages with dynamic /// lengths and messages where the get/set messages have different sizes, but /// share the same parameters, such as CfgDAT. /// @@ -43,14 +43,14 @@ namespace ublox { /// -/// @brief Serializes the CfgDAT message which has a different length for +/// @brief Serializes the CfgDAT message which has a different length for /// get/set. /// template struct Serializer > { - typedef boost::call_traits > + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.datumNum); @@ -72,7 +72,7 @@ struct Serializer > { return 44; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); // ignores datumNum & datumName @@ -96,7 +96,7 @@ struct Serializer > { typedef ublox_msgs::CfgGNSS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.msgVer); @@ -104,7 +104,7 @@ struct Serializer > { stream.next(m.numTrkChUse); stream.next(m.numConfigBlocks); m.blocks.resize(m.numConfigBlocks); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for(std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::deserialize(stream, m.blocks[i]); } @@ -112,7 +112,7 @@ struct Serializer > { return 4 + 8 * m.numConfigBlocks; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.blocks.size() != m.numConfigBlocks) { ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); @@ -123,7 +123,7 @@ struct Serializer > { stream.next(m.numTrkChUse); stream.next( static_cast(m.blocks.size())); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for(std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -133,7 +133,7 @@ struct Serializer > { /// template struct Serializer > { - typedef boost::call_traits > + typedef boost::call_traits > CallTraits; static void read(const uint8_t *data, uint32_t count, @@ -141,7 +141,7 @@ struct Serializer > { ros::serialization::IStream stream(const_cast(data), count); int num_blocks = count / 10; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for(std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } @@ -149,10 +149,10 @@ struct Serializer > { return 10 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for(std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -163,8 +163,8 @@ struct Serializer > { template struct Serializer > { typedef boost::call_traits > CallTraits; - - static void read(const uint8_t *data, uint32_t count, + + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); m.str.resize(count); @@ -176,10 +176,10 @@ struct Serializer > { return m.str.size(); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.str.size(); ++i) + for(std::size_t i = 0; i < m.str.size(); ++i) ros::serialization::serialize(stream, m.str[i]); } }; @@ -192,7 +192,7 @@ struct Serializer > { typedef ublox_msgs::MonVER_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.swVersion); @@ -213,12 +213,12 @@ struct Serializer > { return 40 + (30 * m.extension.size()); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.swVersion); stream.next(m.hwVersion); - for(std::size_t i = 0; i < m.extension.size(); ++i) + for(std::size_t i = 0; i < m.extension.size(); ++i) ros::serialization::serialize(stream, m.extension[i]); } }; @@ -231,7 +231,7 @@ struct Serializer > { typedef ublox_msgs::NavDGPS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -242,7 +242,7 @@ struct Serializer > { stream.next(m.status); stream.next(m.reserved1); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -250,7 +250,7 @@ struct Serializer > { return 16 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.numCh) { ROS_ERROR("NavDGPS numCh must equal sv size"); @@ -263,7 +263,7 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -277,7 +277,7 @@ struct Serializer > { typedef ublox_msgs::NavSBAS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -288,7 +288,7 @@ struct Serializer > { stream.next(m.cnt); stream.next(m.reserved0); m.sv.resize(m.cnt); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -296,7 +296,7 @@ struct Serializer > { return 12 + 12 * m.cnt; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.cnt) { ROS_ERROR("NavSBAS cnt must equal sv size"); @@ -309,7 +309,7 @@ struct Serializer > { stream.next(m.service); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -322,7 +322,7 @@ struct Serializer > { typedef ublox_msgs::NavSAT_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -330,7 +330,7 @@ struct Serializer > { stream.next(m.numSvs); stream.next(m.reserved0); m.sv.resize(m.numSvs); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -338,7 +338,7 @@ struct Serializer > { return 8 + 12 * m.numSvs; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.numSvs) { ROS_ERROR("NavSAT numSvs must equal sv size"); @@ -348,7 +348,7 @@ struct Serializer > { stream.next(m.version); stream.next(static_cast(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -361,7 +361,7 @@ struct Serializer > { typedef ublox_msgs::NavSVINFO_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -369,7 +369,7 @@ struct Serializer > { stream.next(m.globalFlags); stream.next(m.reserved2); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -377,7 +377,7 @@ struct Serializer > { return 8 + 12 * m.numCh; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.numCh) { ROS_ERROR("NavSVINFO numCh must equal sv size"); @@ -387,7 +387,7 @@ struct Serializer > { stream.next(static_cast(m.sv.size())); stream.next(m.globalFlags); stream.next(m.reserved2); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -400,7 +400,7 @@ struct Serializer > { typedef ublox_msgs::RxmRAW_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); @@ -408,7 +408,7 @@ struct Serializer > { stream.next(m.numSV); stream.next(m.reserved1); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -416,7 +416,7 @@ struct Serializer > { return 8 + 24 * m.numSV; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.numSV) { ROS_ERROR("RxmRAW numSV must equal sv size"); @@ -426,7 +426,7 @@ struct Serializer > { stream.next(m.week); stream.next(static_cast(m.sv.size())); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -439,7 +439,7 @@ struct Serializer > { typedef ublox_msgs::RxmRAWX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); @@ -450,7 +450,7 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.meas.resize(m.numMeas); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for(std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::deserialize(stream, m.meas[i]); } @@ -458,7 +458,7 @@ struct Serializer > { return 16 + 32 * m.numMeas; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.meas.size() != m.numMeas) { ROS_ERROR("RxmRAWX numMeas must equal meas size"); @@ -471,7 +471,7 @@ struct Serializer > { stream.next(m.recStat); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for(std::size_t i = 0; i < m.meas.size(); ++i) ros::serialization::serialize(stream, m.meas[i]); } }; @@ -484,7 +484,7 @@ struct Serializer > { typedef ublox_msgs::RxmSFRBX_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.gnssId); @@ -496,7 +496,7 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.dwrd.resize(m.numWords); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for(std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::deserialize(stream, m.dwrd[i]); } @@ -504,7 +504,7 @@ struct Serializer > { return 8 + 4 * m.numWords; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.dwrd.size() != m.numWords) { ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); @@ -518,7 +518,7 @@ struct Serializer > { stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for(std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -531,7 +531,7 @@ struct Serializer > { typedef ublox_msgs::RxmSVSI_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -539,7 +539,7 @@ struct Serializer > { stream.next(m.numVis); stream.next(m.numSV); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::deserialize(stream, m.sv[i]); } @@ -547,7 +547,7 @@ struct Serializer > { return 8 + 6 * m.numSV; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sv.size() != m.numSV) { ROS_ERROR("RxmSVSI numSV must equal sv size"); @@ -557,7 +557,7 @@ struct Serializer > { stream.next(m.week); stream.next(m.numVis); stream.next(static_cast(m.sv.size())); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for(std::size_t i = 0; i < m.sv.size(); ++i) ros::serialization::serialize(stream, m.sv[i]); } }; @@ -570,7 +570,7 @@ struct Serializer > { typedef ublox_msgs::RxmALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); @@ -591,12 +591,12 @@ struct Serializer > { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for(std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -610,7 +610,7 @@ struct Serializer > typedef ublox_msgs::RxmEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); @@ -646,16 +646,16 @@ struct Serializer > return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for(std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for(std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for(std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -668,7 +668,7 @@ struct Serializer > { typedef ublox_msgs::AidALM_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); @@ -682,19 +682,19 @@ struct Serializer > { stream.next(temp); m.dwrd.push_back(temp); } - } + } } static uint32_t serializedLength (typename CallTraits::param_type m) { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for(std::size_t i = 0; i < m.dwrd.size(); ++i) ros::serialization::serialize(stream, m.dwrd[i]); } }; @@ -708,7 +708,7 @@ struct Serializer > typedef ublox_msgs::AidEPH_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); @@ -743,16 +743,16 @@ struct Serializer > return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for(std::size_t i = 0; i < m.sf1d.size(); ++i) ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + for(std::size_t i = 0; i < m.sf2d.size(); ++i) ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + for(std::size_t i = 0; i < m.sf3d.size(); ++i) ros::serialization::serialize(stream, m.sf3d[i]); } }; @@ -763,10 +763,10 @@ struct Serializer > /// template struct Serializer > { - typedef boost::call_traits > + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.timeTag); @@ -777,7 +777,7 @@ struct Serializer > { int data_size = (count - (calib_valid ? 12 : 8)) / 4; // Repeating block m.data.resize(data_size); - for(std::size_t i = 0; i < data_size; ++i) + for(std::size_t i = 0; i < data_size; ++i) ros::serialization::deserialize(stream, m.data[i]); // Optional block if(calib_valid) { @@ -790,15 +790,15 @@ struct Serializer > { return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); - for(std::size_t i = 0; i < m.data.size(); ++i) + for(std::size_t i = 0; i < m.data.size(); ++i) ros::serialization::serialize(stream, m.data[i]); - for(std::size_t i = 0; i < m.calibTtag.size(); ++i) + for(std::size_t i = 0; i < m.calibTtag.size(); ++i) ros::serialization::serialize(stream, m.calibTtag[i]); } }; @@ -808,17 +808,17 @@ struct Serializer > { /// template struct Serializer > { - typedef boost::call_traits > + typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.reserved0); m.blocks.clear(); int num_blocks = (count - 4) / 8; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for(std::size_t i = 0; i < num_blocks; ++i) ros::serialization::deserialize(stream, m.blocks[i]); } @@ -827,11 +827,11 @@ struct Serializer > { return 4 + 8 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { ros::serialization::OStream stream(data, size); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for(std::size_t i = 0; i < m.blocks.size(); ++i) ros::serialization::serialize(stream, m.blocks[i]); } }; @@ -844,7 +844,7 @@ struct Serializer > { typedef ublox_msgs::EsfSTATUS_ Msg; typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename CallTraits::reference m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); @@ -853,7 +853,7 @@ struct Serializer > { stream.next(m.reserved2); stream.next(m.numSens); m.sens.resize(m.numSens); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for(std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::deserialize(stream, m.sens[i]); } @@ -861,7 +861,7 @@ struct Serializer > { return 16 + 4 * m.numSens; } - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename CallTraits::param_type m) { if(m.sens.size() != m.numSens) { ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); @@ -872,7 +872,7 @@ struct Serializer > { stream.next(m.fusionMode); stream.next(m.reserved2); stream.next(static_cast(m.sens.size())); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for(std::size_t i = 0; i < m.sens.size(); ++i) ros::serialization::serialize(stream, m.sens[i]); } }; diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index 990007d3..a710a374 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -112,40 +112,40 @@ namespace ublox_msgs { namespace Class { - static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, - //!< Speed, Time, Acceleration, Heading, + static const uint8_t NAV = 0x01; //!< Navigation Result Messages: Position, + //!< Speed, Time, Acceleration, Heading, //!< DOP, SVs used - static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: + static const uint8_t RXM = 0x02; //!< Receiver Manager Messages: //!< Satellite Status, RTC Status - static const uint8_t INF = 0x04; //!< Information Messages: + static const uint8_t INF = 0x04; //!< Information Messages: //!< Printf-Style Messages, with IDs such as //!< Error, Warning, Notice - static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject + static const uint8_t ACK = 0x05; //!< Ack/Nack Messages: Acknowledge or Reject //!< messages to CFG input messages - static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set - //!< Dynamic Model, Set DOP Mask, Set Baud + static const uint8_t CFG = 0x06; //!< Configuration Input Messages: Set + //!< Dynamic Model, Set DOP Mask, Set Baud //!< Rate, etc. - static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. - //!< Memory/Flash erase/write, Reboot, Flash + static const uint8_t UPD = 0x09; //!< Firmware Update Messages: i.e. + //!< Memory/Flash erase/write, Reboot, Flash //!< identification, etc. - //!< Used to update the firmware and identify + //!< Used to update the firmware and identify //!< any attached flash device - static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication - //!< Status, CPU Load, Stack Usage, + static const uint8_t MON = 0x0A; //!< Monitoring Messages: Communication + //!< Status, CPU Load, Stack Usage, //!< Task Status - static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, + static const uint8_t AID = 0x0B; //!< AssistNow Aiding Messages: Ephemeris, //!< Almanac, other A-GPS data input - static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, + static const uint8_t TIM = 0x0D; //!< Timing Messages: Timepulse Output, //!< Timemark Results - static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: - //!< External sensor measurements and status + static const uint8_t ESF = 0x10; //!< External Sensor Fusion Messages: + //!< External sensor measurements and status //!< information - static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: + static const uint8_t MGA = 0x13; //!< Multiple GNSS Assistance Messages: //!< Assistance data for various GNSS - static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, + static const uint8_t LOG = 0x21; //!< Logging Messages: Log creation, //!< deletion, info and retrieval static const uint8_t SEC = 0x27; //!< Security Feature Messages - static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: + static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: //!< High rate time, position, speed, heading static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages } @@ -193,8 +193,8 @@ namespace Message { } namespace ACK { - static const uint8_t NACK = 0x00; - static const uint8_t ACK = 0x01; + static const uint8_t NACK = 0x00; + static const uint8_t ACK = 0x01; } namespace AID { @@ -227,7 +227,7 @@ namespace Message { //! SOS and SOS_Ack have the same message ID, but different lengths static const uint8_t SOS = UpdSOS::MESSAGE_ID; } - + namespace MON { static const uint8_t GNSS = MonGNSS::MESSAGE_ID; static const uint8_t HW = MonHW::MESSAGE_ID; diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index f94bb70a..7c4e5d18 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -31,167 +31,167 @@ template std::vector > ublox::Message::keys_; -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, ublox_msgs, NavATT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, ublox_msgs, NavCLOCK); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, ublox_msgs, NavDGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, ublox_msgs, NavDOP); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, ublox_msgs, NavPOSECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, ublox_msgs, NavPOSLLH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, - ublox_msgs::Message::NAV::RELPOSNED, - ublox_msgs, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, + ublox_msgs::Message::NAV::RELPOSNED, + ublox_msgs, NavRELPOSNED); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED9, ublox_msgs, NavRELPOSNED9); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, ublox_msgs, NavSBAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, ublox_msgs, NavSOL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, ublox_msgs, NavPVT7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, ublox_msgs, NavSAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, ublox_msgs, NavSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, ublox_msgs, NavSVIN); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, ublox_msgs, NavSVINFO); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, ublox_msgs, NavTIMEGPS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, ublox_msgs, NavTIMEUTC); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, ublox_msgs, NavVELECEF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, ublox_msgs, NavVELNED); -// ACK messages are declared differently because they both have the same +// ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, ublox_msgs, Ack); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, ublox_msgs, Ack, ACK); -// INF messages are declared differently because they all have the same +// INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, ublox_msgs, Inf); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::WARNING, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::WARNING, ublox_msgs, Inf, WARNING); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::NOTICE, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::NOTICE, ublox_msgs, Inf, NOTICE); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::TEST, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::TEST, ublox_msgs, Inf, TEST); -DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, - ublox_msgs::Message::INF::DEBUG, +DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, + ublox_msgs::Message::INF::DEBUG, ublox_msgs, Inf, DEBUG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, ublox_msgs, RxmALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, ublox_msgs, RxmEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, ublox_msgs, RxmRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, ublox_msgs, RxmRAWX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, ublox_msgs, RxmRTCM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, ublox_msgs, RxmSFRB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, ublox_msgs, RxmSFRBX); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, ublox_msgs, RxmSVSI); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, ublox_msgs, CfgANT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, ublox_msgs, CfgCFG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, ublox_msgs, CfgDAT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, ublox_msgs, CfgDGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, ublox_msgs, CfgGNSS); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, ublox_msgs, CfgHNR); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, ublox_msgs, CfgINF); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, ublox_msgs, CfgMSG); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, ublox_msgs, CfgNAV5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, ublox_msgs, CfgNAVX5); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, ublox_msgs, CfgNMEA7); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, ublox_msgs, CfgPRT); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, ublox_msgs, CfgRATE); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, ublox_msgs, CfgRST); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, ublox_msgs, CfgTMODE3); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, ublox_msgs, CfgUSB); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS); // SOS and SOS_Ack have the same message ID, but different lengths -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS_Ack); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, ublox_msgs, MonGNSS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, ublox_msgs, MonHW6); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, ublox_msgs, MonVER); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, ublox_msgs, AidALM); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, ublox_msgs, AidEPH); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, ublox_msgs, AidHUI); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, ublox_msgs, EsfINS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, ublox_msgs, EsfMEAS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, ublox_msgs, EsfRAW); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, ublox_msgs, EsfSTATUS); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, ublox_msgs, MgaGAL); -DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, ublox_msgs, HnrPVT); // TIM messages diff --git a/ublox_serialization/include/ublox/checksum.hpp b/ublox_serialization/include/ublox/checksum.hpp index 8c9d3adb..e35f4822 100644 --- a/ublox_serialization/include/ublox/checksum.hpp +++ b/ublox_serialization/include/ublox/checksum.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -35,14 +35,14 @@ namespace ublox { /** * @brief calculate the checksum of a u-blox_message - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param ck_a the checksum a output * @param ck_b the checksum b output */ -static inline void calculateChecksum(const uint8_t *data, - uint32_t size, - uint8_t &ck_a, +static inline void calculateChecksum(const uint8_t *data, + uint32_t size, + uint8_t &ck_a, uint8_t &ck_b) { ck_a = 0; ck_b = 0; for(uint32_t i = 0; i < size; ++i) @@ -54,13 +54,13 @@ static inline void calculateChecksum(const uint8_t *data, /** * @brief calculate the checksum of a u-blox_message. - * @param data the start of the u-blox message + * @param data the start of the u-blox message * @param data the size of the u-blox message * @param checksum the checksum output * @return the checksum */ -static inline uint16_t calculateChecksum(const uint8_t *data, - uint32_t size, +static inline uint16_t calculateChecksum(const uint8_t *data, + uint32_t size, uint16_t &checksum) { uint8_t *byte = reinterpret_cast(&checksum); calculateChecksum(data, size, byte[0], byte[1]); diff --git a/ublox_serialization/include/ublox/serialization.hpp b/ublox_serialization/include/ublox/serialization.hpp index e0984c69..dbb6caa8 100644 --- a/ublox_serialization/include/ublox/serialization.hpp +++ b/ublox_serialization/include/ublox/serialization.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -39,8 +39,8 @@ /// /// This file defines the Serializer template class which encodes and decodes -/// specific message types. -/// The Reader class decodes messages and from a buffer and the Writer class +/// specific message types. +/// The Reader class decodes messages and from a buffer and the Writer class /// encodes messages and writes them to a buffer. /// It also declares macros for declaring Messages. The Message class /// maps ROS messages types to class and message ID(s). @@ -54,13 +54,13 @@ namespace ublox { //! u-blox message Sync A char -static const uint8_t DEFAULT_SYNC_A = 0xB5; +static const uint8_t DEFAULT_SYNC_A = 0xB5; //! u-blox message Sync B char -static const uint8_t DEFAULT_SYNC_B = 0x62; +static const uint8_t DEFAULT_SYNC_B = 0x62; //! Number of bytes in a message header (Sync chars + class ID + message ID) -static const uint8_t kHeaderLength = 6; +static const uint8_t kHeaderLength = 6; //! Number of checksum bytes in the u-blox message -static const uint8_t kChecksumLength = 2; +static const uint8_t kChecksumLength = 2; /** * @brief Encodes and decodes messages. @@ -73,25 +73,25 @@ struct Serializer { * @param count the number of bytes in the message payload * @param message the output message */ - static void read(const uint8_t *data, uint32_t count, + static void read(const uint8_t *data, uint32_t count, typename boost::call_traits::reference message); /** * @brief Get the length of the message payload in bytes. - * + * * @details The payload does not include the header or checksum. * @param message the message to get the length of * @return the length of the message in bytes. */ static uint32_t serializedLength( typename boost::call_traits::param_type message); - + /** * @brief Encode the message payload as a byte array. * @param data a buffer to fill with the message payload bytes * @param size the length of the buffer * @param message the output message */ - static void write(uint8_t *data, uint32_t size, + static void write(uint8_t *data, uint32_t size, typename boost::call_traits::param_type message); }; @@ -109,12 +109,12 @@ class Message { * @return whether or not this message type decode the u-blox message */ static bool canDecode(uint8_t class_id, uint8_t message_id) { - return std::find(keys_.begin(), keys_.end(), + return std::find(keys_.begin(), keys_.end(), std::make_pair(class_id, message_id)) != keys_.end(); } - + /** - * @brief Indicate that this message type can decode u-blox messages with the + * @brief Indicate that this message type can decode u-blox messages with the * given ID * @param class_id the class ID of the u-blox message * @param message_id the message ID of the u-blox message @@ -125,8 +125,8 @@ class Message { struct StaticKeyInitializer { - StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { - Message::addKey(class_id, message_id); + StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { + Message::addKey(class_id, message_id); } }; @@ -141,27 +141,27 @@ struct Options { /** * The default options for a u-blox message. */ - Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), + Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), header_length(kHeaderLength), checksum_length(kChecksumLength) {} //! The sync_a byte value identifying the start of a message - uint8_t sync_a; + uint8_t sync_a; //! The sync_b byte value identifying the start of a message - uint8_t sync_b; + uint8_t sync_b; //! The length of the message header in bytes (everything before the payload) - uint8_t header_length; + uint8_t header_length; //! The length of the checksum in bytes - uint8_t checksum_length; - + uint8_t checksum_length; + /** * @brief Get the number of bytes in the header and footer. * @return the number of bytes in the header and footer */ int wrapper_length() { - return header_length + checksum_length; + return header_length + checksum_length; } }; -/** +/** * @brief Decodes byte messages into u-blox ROS messages. */ class Reader { @@ -169,11 +169,11 @@ class Reader { /** * @param data a buffer containing u-blox messages * @param count the size of the buffer - * @param options A struct containing the parameters sync_a and sync_b + * @param options A struct containing the parameters sync_a and sync_b * which represent the sync bytes indicating the beginning of the message */ - Reader(const uint8_t *data, uint32_t count, - const Options &options = Options()) : + Reader(const uint8_t *data, uint32_t count, + const Options &options = Options()) : data_(data), count_(count), found_(false), options_(options) {} typedef const uint8_t *iterator; @@ -188,8 +188,8 @@ class Reader { // Search for a message header for( ; count_ > 0; --count_, ++data_) { - if (data_[0] == options_.sync_a && - (count_ == 1 || data_[1] == options_.sync_b)) + if (data_[0] == options_.sync_a && + (count_ == 1 || data_[1] == options_.sync_b)) break; } @@ -206,7 +206,7 @@ class Reader { // Verify message is long enough to have sync chars, id, length & checksum if (count_ < options_.wrapper_length()) return false; // Verify the header bits - if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) + if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) return false; // Verify that the buffer length is long enough based on the received // message length @@ -217,10 +217,10 @@ class Reader { } /** - * @brief Go to the start of the next message based on the received message + * @brief Go to the start of the next message based on the received message * length. * - * @details Warning: Does not go to the correct byte location if the received + * @details Warning: Does not go to the correct byte location if the received * message length is incorrect. If this is the case, search must be called. */ iterator next() { @@ -256,15 +256,15 @@ class Reader { */ uint32_t length() { return (data_[5] << 8) + data_[4]; } const uint8_t *data() { return data_ + options_.header_length; } - + /** * @brief Get the checksum of the u-blox message. * * @return the checksum of the u-blox message */ - uint16_t checksum() { + uint16_t checksum() { return *reinterpret_cast(data_ + options_.header_length + - length()); + length()); } /** @@ -273,16 +273,16 @@ class Reader { * @param search whether or not to skip to the next message in the buffer */ template - bool read(typename boost::call_traits::reference message, + bool read(typename boost::call_traits::reference message, bool search = false) { if (search) this->search(); - if (!found()) return false; + if (!found()) return false; if (!Message::canDecode(classId(), messageId())) return false; uint16_t chk; if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { // checksum error - ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), + ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), messageId()); return false; } @@ -293,10 +293,10 @@ class Reader { /** * @brief Can the given message type decode the current message in the buffer? - * @return whether the given message type can decode the current message in + * @return whether the given message type can decode the current message in * the buffer */ - template + template bool hasType() { if (!found()) return false; return Message::canDecode(classId(), messageId()); @@ -304,7 +304,7 @@ class Reader { /** * @brief Does the u-blox message have the given class and message ID? - * @return Whether or not the u-blox message has the given class and message + * @return Whether or not the u-blox message has the given class and message * ID */ bool isMessage(uint8_t class_id, uint8_t message_id) { @@ -314,16 +314,16 @@ class Reader { private: //! The buffer of message bytes - const uint8_t *data_; + const uint8_t *data_; //! the number of bytes in the buffer, //! decrement as the buffer is read - uint32_t count_; + uint32_t count_; //! Whether or not a message has been found - bool found_; + bool found_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; -/** +/** * @brief Encodes a u-blox ROS message as a byte array. */ class Writer { @@ -336,7 +336,7 @@ class Writer { * @param size the size of the buffer * @param options options representing the message sync chars, etc. */ - Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : + Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : data_(data), size_(size), options_(options) {} /** @@ -346,18 +346,18 @@ class Writer { * @param message_id the u-blox message ID, defaults to the message MESSAGE_ID * @return true if the message was encoded correctly, false otherwise */ - template bool write(const T& message, - uint8_t class_id = T::CLASS_ID, + template bool write(const T& message, + uint8_t class_id = T::CLASS_ID, uint8_t message_id = T::MESSAGE_ID) { // Check for buffer overflow uint32_t length = Serializer::serializedLength(message); if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } // Encode the message and add it to the buffer - Serializer::write(data_ + options_.header_length, + Serializer::write(data_ + options_.header_length, size_ - options_.header_length, message); return write(0, length, class_id, message_id); } @@ -371,10 +371,10 @@ class Writer { * @param message_id the u-blox message ID * @return true if the message was encoded correctly, false otherwise */ - bool write(const uint8_t* message, uint32_t length, uint8_t class_id, + bool write(const uint8_t* message, uint32_t length, uint8_t class_id, uint8_t message_id) { if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } @@ -410,12 +410,12 @@ class Writer { private: //! The buffer of message bytes - iterator data_; + iterator data_; //! The number of remaining bytes in the buffer /*! Decrements as messages are written to the buffer */ - uint32_t size_; + uint32_t size_; //! Options representing the sync char values, etc. - Options options_; + Options options_; }; } // namespace ublox diff --git a/ublox_serialization/include/ublox/serialization_ros.hpp b/ublox_serialization/include/ublox/serialization_ros.hpp index 6b32f65b..d0ff6aa4 100644 --- a/ublox_serialization/include/ublox/serialization_ros.hpp +++ b/ublox_serialization/include/ublox/serialization_ros.hpp @@ -14,9 +14,9 @@ // 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 +// 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 THE COPYRIGHT HOLDER BE LIABLE FOR ANY // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; @@ -37,7 +37,7 @@ namespace ublox { template -void Serializer::read(const uint8_t *data, uint32_t count, +void Serializer::read(const uint8_t *data, uint32_t count, typename boost::call_traits::reference message) { ros::serialization::IStream stream(const_cast(data), count); ros::serialization::Serializer::read(stream, message); @@ -50,7 +50,7 @@ uint32_t Serializer::serializedLength( } template -void Serializer::write(uint8_t *data, uint32_t size, +void Serializer::write(uint8_t *data, uint32_t size, typename boost::call_traits::param_type message) { ros::serialization::OStream stream(data, size); ros::serialization::Serializer::write(stream, message); From 65338b22b154d7224e96b49a963fce62321b80ca Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 10:38:20 -0500 Subject: [PATCH 003/152] Switch out boost time and mutex for std:: Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 39 ++++++++++---------- ublox_gps/include/ublox_gps/callback.hpp | 32 +++++++++------- ublox_gps/include/ublox_gps/gps.hpp | 25 +++++++------ ublox_gps/include/ublox_gps/worker.hpp | 10 +++-- ublox_gps/src/gps.cpp | 25 +++++++------ ublox_gps/src/node.cpp | 4 +- 6 files changed, 73 insertions(+), 62 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index b4cf086b..2ac6fb5e 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -26,8 +26,12 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_ASYNC_WORKER_H -#define UBLOX_GPS_ASYNC_WORKER_H +#ifndef UBLOX_GPS_ASYNC_WORKER_HPP +#define UBLOX_GPS_ASYNC_WORKER_HPP + +#include +#include +#include #include @@ -50,9 +54,6 @@ int debug; //!< Used to determine which debug messages to display template class AsyncWorker : public Worker { public: - typedef boost::mutex Mutex; - typedef boost::mutex::scoped_lock ScopedLock; - /** * @brief Construct an Asynchronous I/O worker. * @param stream the stream for th I/O service @@ -86,7 +87,7 @@ class AsyncWorker : public Worker { * @brief Wait for incoming messages. * @param timeout the maximum time to wait */ - void wait(const boost::posix_time::time_duration& timeout); + void wait(const std::chrono::milliseconds& timeout); bool isOpen() const { return stream_->is_open(); } @@ -116,14 +117,14 @@ class AsyncWorker : public Worker { boost::shared_ptr stream_; //!< The I/O stream boost::shared_ptr io_service_; //!< The I/O service - Mutex read_mutex_; //!< Lock for the input buffer - boost::condition read_condition_; + std::mutex read_mutex_; //!< Lock for the input buffer + std::condition_variable read_condition_; std::vector in_; //!< The input buffer std::size_t in_buffer_size_; //!< number of bytes currently in the input //!< buffer - Mutex write_mutex_; //!< Lock for the output buffer - boost::condition write_condition_; + std::mutex write_mutex_; //!< Lock for the output buffer + std::condition_variable write_condition_; std::vector out_; //!< The output buffer boost::shared_ptr background_thread_; //!< thread for the I/O @@ -161,7 +162,7 @@ AsyncWorker::~AsyncWorker() { template bool AsyncWorker::send(const unsigned char* data, const unsigned int size) { - ScopedLock lock(write_mutex_); + std::lock_guard lock(write_mutex_); if(size == 0) { ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); return true; @@ -179,7 +180,7 @@ bool AsyncWorker::send(const unsigned char* data, template void AsyncWorker::doWrite() { - ScopedLock lock(write_mutex_); + std::lock_guard lock(write_mutex_); // Do nothing if out buffer is empty if (out_.size() == 0) { return; @@ -202,7 +203,7 @@ void AsyncWorker::doWrite() { template void AsyncWorker::doRead() { - ScopedLock lock(read_mutex_); + std::lock_guard lock(read_mutex_); stream_->async_read_some( boost::asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), @@ -214,7 +215,7 @@ void AsyncWorker::doRead() { template void AsyncWorker::readEnd(const boost::system::error_code& error, std::size_t bytes_transfered) { - ScopedLock lock(read_mutex_); + std::lock_guard lock(read_mutex_); if (error) { ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", error.message().c_str(), @@ -250,7 +251,7 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, template void AsyncWorker::doClose() { - ScopedLock lock(read_mutex_); + std::lock_guard lock(read_mutex_); stopping_ = true; boost::system::error_code error; stream_->close(error); @@ -261,11 +262,11 @@ void AsyncWorker::doClose() { template void AsyncWorker::wait( - const boost::posix_time::time_duration& timeout) { - ScopedLock lock(read_mutex_); - read_condition_.timed_wait(lock, timeout); + const std::chrono::milliseconds& timeout) { + std::unique_lock lock(read_mutex_); + read_condition_.wait_for(lock, timeout); } } // namespace ublox_gps -#endif // UBLOX_GPS_ASYNC_WORKER_H +#endif // UBLOX_GPS_ASYNC_WORKER_HPP diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 87eb6f89..73036203 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -26,8 +26,12 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_CALLBACK_H -#define UBLOX_GPS_CALLBACK_H +#ifndef UBLOX_GPS_CALLBACK_HPP +#define UBLOX_GPS_CALLBACK_HPP + +#include +#include +#include #include #include @@ -50,14 +54,14 @@ class CallbackHandler { /** * @brief Wait for on the condition. */ - bool wait(const boost::posix_time::time_duration& timeout) { - boost::mutex::scoped_lock lock(mutex_); - return condition_.timed_wait(lock, timeout); + bool wait(const std::chrono::milliseconds& timeout) { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout) == std::cv_status::no_timeout; } protected: - boost::mutex mutex_; //!< Lock for the handler - boost::condition_variable condition_; //!< Condition for the handler lock + std::mutex mutex_; //!< Lock for the handler + std::condition_variable condition_; //!< Condition for the handler lock }; /** @@ -85,7 +89,7 @@ class CallbackHandler_ : public CallbackHandler { * @param reader a reader to decode the message buffer */ void handle(ublox::Reader& reader) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); try { if (!reader.read(message_)) { ROS_DEBUG_COND(debug >= 2, @@ -127,7 +131,7 @@ class CallbackHandlers { */ template void insert(typename CallbackHandler_::Callback callback) { - boost::mutex::scoped_lock lock(callback_mutex_); + std::lock_guard lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), @@ -146,7 +150,7 @@ class CallbackHandlers { void insert( typename CallbackHandler_::Callback callback, unsigned int message_id) { - boost::mutex::scoped_lock lock(callback_mutex_); + std::lock_guard lock(callback_mutex_); CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, message_id), @@ -159,7 +163,7 @@ class CallbackHandlers { */ void handle(ublox::Reader& reader) { // Find the callback handlers for the message & decode it - boost::mutex::scoped_lock lock(callback_mutex_); + std::lock_guard lock(callback_mutex_); Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId()); for (Callbacks::iterator callback = callbacks_.lower_bound(key); @@ -173,7 +177,7 @@ class CallbackHandlers { * @param timeout the amount of time to wait for the desired message */ template - bool read(T& message, const boost::posix_time::time_duration& timeout) { + bool read(T& message, const std::chrono::milliseconds& timeout) { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); @@ -230,9 +234,9 @@ class CallbackHandlers { // Call back handlers for u-blox messages Callbacks callbacks_; - boost::mutex callback_mutex_; + std::mutex callback_mutex_; }; } // namespace ublox_gps -#endif // UBLOX_GPS_CALLBACK_H +#endif // UBLOX_GPS_CALLBACK_HPP diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 268e01b8..d3881282 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -27,9 +27,10 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_H -#define UBLOX_GPS_H +#ifndef UBLOX_GPS_GPS_HPP +#define UBLOX_GPS_GPS_HPP // STL +#include #include #include #include @@ -68,7 +69,7 @@ constexpr static unsigned int kBaudrates[] = { 4800, class Gps { public: //! Sleep time [ms] after setting the baudrate - constexpr static int kSetBaudrateSleepMs = 500; + int kSetBaudrateSleepMs = 500; //! Default timeout for ACK messages in seconds constexpr static double kDefaultAckTimeout = 1.0; //! Size of write buffer for output messages @@ -127,7 +128,7 @@ class Gps { * @brief Reset I/O communications. * @param wait Time to wait before restarting communications */ - void reset(const boost::posix_time::time_duration& wait); + void reset(const std::chrono::milliseconds& wait); /** * @brief Send a reset message to the u-blox device. @@ -145,7 +146,7 @@ class Gps { * I/O reset successfully */ bool configGnss(ublox_msgs::CfgGNSS gnss, - const boost::posix_time::time_duration& wait); + const std::chrono::milliseconds& wait); /** * @brief Send a message to the receiver to delete the BBR data stored in @@ -351,7 +352,7 @@ class Gps { */ template bool read(T& message, - const boost::posix_time::time_duration& timeout = default_timeout_); + const std::chrono::milliseconds& timeout = default_timeout_); bool isInitialized() const { return worker_ != 0; } bool isConfigured() const { return isInitialized() && configured_; } @@ -367,7 +368,7 @@ class Gps { template bool poll(ConfigT& message, const std::vector& payload = std::vector(), - const boost::posix_time::time_duration& timeout = default_timeout_); + const std::chrono::milliseconds& timeout = default_timeout_); /** * Poll a u-blox message. * @param class_id the u-blox message class id @@ -396,7 +397,7 @@ class Gps { * @param msg_id the expected message ID of the ACK * @return true if expected ACK received, false otherwise */ - bool waitForAcknowledge(const boost::posix_time::time_duration& timeout, + bool waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id); /** @@ -472,7 +473,7 @@ class Gps { //! The default timeout for ACK messages - static const boost::posix_time::time_duration default_timeout_; + static const std::chrono::milliseconds default_timeout_; //! Stores last received ACK accessed by multiple threads mutable boost::atomic ack_; @@ -503,13 +504,13 @@ void Gps::subscribeId(typename CallbackHandler_::Callback callback, template bool Gps::poll(ConfigT& message, const std::vector& payload, - const boost::posix_time::time_duration& timeout) { + const std::chrono::milliseconds& timeout) { if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; return read(message, timeout); } template -bool Gps::read(T& message, const boost::posix_time::time_duration& timeout) { +bool Gps::read(T& message, const std::chrono::milliseconds& timeout) { if (!worker_) return false; return callbacks_.read(message, timeout); } @@ -544,4 +545,4 @@ bool Gps::configure(const ConfigT& message, bool wait) { } // namespace ublox_gps -#endif // UBLOX_GPS_H +#endif // UBLOX_GPS_GPS_HPP diff --git a/ublox_gps/include/ublox_gps/worker.hpp b/ublox_gps/include/ublox_gps/worker.hpp index 661274a3..d3964bf2 100644 --- a/ublox_gps/include/ublox_gps/worker.hpp +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -26,8 +26,10 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_GPS_WORKER_H -#define UBLOX_GPS_WORKER_H +#ifndef UBLOX_GPS_WORKER_HPP +#define UBLOX_GPS_WORKER_HPP + +#include #include #include @@ -65,7 +67,7 @@ class Worker { * @brief Wait for an incoming message. * @param timeout the maximum time to wait. */ - virtual void wait(const boost::posix_time::time_duration& timeout) = 0; + virtual void wait(const std::chrono::milliseconds& timeout) = 0; /** * @brief Whether or not the I/O stream is open. @@ -75,4 +77,4 @@ class Worker { } // namespace ublox_gps -#endif // UBLOX_GPS_WORKER_H +#endif // UBLOX_GPS_WORKER_HPP diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 8ac95fc5..dad5d19d 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -27,6 +27,9 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== +#include +#include + #include #include @@ -34,8 +37,8 @@ namespace ublox_gps { using namespace ublox_msgs; -const boost::posix_time::time_duration Gps::default_timeout_ = - boost::posix_time::milliseconds( +const std::chrono::milliseconds Gps::default_timeout_ = + std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); Gps::Gps() : configured_(false), config_on_startup_flag_(true) { @@ -151,8 +154,8 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, continue; serial->set_option( boost::asio::serial_port_base::baud_rate(kBaudrates[i])); - boost::this_thread::sleep( - boost::posix_time::milliseconds(kSetBaudrateSleepMs)); + std::this_thread::sleep_for( + std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } @@ -254,11 +257,11 @@ void Gps::close() { configured_ = false; } -void Gps::reset(const boost::posix_time::time_duration& wait) { +void Gps::reset(const std::chrono::milliseconds& wait) { worker_.reset(); configured_ = false; // sleep because of undefined behavior after I/O reset - boost::this_thread::sleep(wait); + std::this_thread::sleep_for(wait); if (host_ == "") resetSerial(port_); else @@ -280,7 +283,7 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { } bool Gps::configGnss(CfgGNSS gnss, - const boost::posix_time::time_duration& wait) { + const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox ROS_DEBUG("Re-configuring GNSS."); if (!configure(gnss)) @@ -537,15 +540,15 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, return true; } -bool Gps::waitForAcknowledge(const boost::posix_time::time_duration& timeout, +bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id) { ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", class_id, msg_id); - boost::posix_time::ptime wait_until( - boost::posix_time::second_clock::local_time() + timeout); + std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); + std::chrono::system_clock::time_point wait_until = now + timeout; Ack ack = ack_.load(boost::memory_order_seq_cst); - while (boost::posix_time::second_clock::local_time() < wait_until + while (std::chrono::system_clock::now() < wait_until && (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 9c3b591f..523f9674 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -424,7 +424,7 @@ bool UbloxNode::configureUblox() { if (load_.loadMask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); - boost::posix_time::seconds wait(kResetWait); + std::chrono::seconds wait(kResetWait); gps.reset(wait); if (!gps.isConfigured()) throw std::runtime_error(std::string("Failed to reset serial I/O") + @@ -1187,7 +1187,7 @@ bool UbloxFirmware8::configureUblox() { // since this requires a cold reset if (correct) ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15))) + else if (!gps.configGnss(cfg_gnss, std::chrono::seconds(15))) throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); From dc2b36483e5f8c6972576fcf473ff448e5a9e54a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 11:05:51 -0500 Subject: [PATCH 004/152] Remove boost from ublox_msgs.hpp serialization. Signed-off-by: Chris Lalancette --- .../ublox/serialization/ublox_msgs.hpp | 371 +++++++++--------- 1 file changed, 175 insertions(+), 196 deletions(-) diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp index d7b2771d..d17b8c94 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp @@ -48,10 +48,8 @@ namespace ublox { /// template struct Serializer > { - typedef boost::call_traits > - CallTraits; static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::CfgDAT_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.datumNum); stream.next(m.datumName); @@ -66,14 +64,14 @@ struct Serializer > { stream.next(m.scale); } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::CfgDAT_ &m) { // this is the size of CfgDAT set messages // serializedLength is only used for writes so this is ok return 44; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::CfgDAT_ &m) { ros::serialization::OStream stream(data, size); // ignores datumNum & datumName stream.next(m.majA); @@ -93,28 +91,26 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::CfgGNSS_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::CfgGNSS_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.msgVer); stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); stream.next(m.numConfigBlocks); m.blocks.resize(m.numConfigBlocks); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::deserialize(stream, m.blocks[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::CfgGNSS_ &m) { return 4 + 8 * m.numConfigBlocks; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.blocks.size() != m.numConfigBlocks) { + const ublox_msgs::CfgGNSS_ &m) { + if (m.blocks.size() != m.numConfigBlocks) { ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); } ros::serialization::OStream stream(data, size); @@ -122,9 +118,10 @@ struct Serializer > { stream.next(m.numTrkChHw); stream.next(m.numTrkChUse); stream.next( - static_cast(m.blocks.size())); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + static_cast::_numConfigBlocks_type>(m.blocks.size())); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::serialize(stream, m.blocks[i]); + } } }; @@ -133,27 +130,26 @@ struct Serializer > { /// template struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::CfgINF_ &m) { ros::serialization::IStream stream(const_cast(data), count); int num_blocks = count / 10; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) { ros::serialization::deserialize(stream, m.blocks[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::CfgINF_ &m) { return 10 * m.blocks.size(); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::CfgINF_ &m) { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::serialize(stream, m.blocks[i]); + } } }; @@ -162,25 +158,25 @@ struct Serializer > { /// template struct Serializer > { - typedef boost::call_traits > CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::Inf_ &m) { ros::serialization::IStream stream(const_cast(data), count); m.str.resize(count); - for (int i = 0; i < count; ++i) + for (int i = 0; i < count; ++i) { ros::serialization::deserialize(stream, m.str[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::Inf_ &m) { return m.str.size(); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::Inf_ &m) { ros::serialization::OStream stream(data, size); - for(std::size_t i = 0; i < m.str.size(); ++i) + for (std::size_t i = 0; i < m.str.size(); ++i) { ros::serialization::serialize(stream, m.str[i]); + } } }; @@ -189,11 +185,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::MonVER_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::MonVER_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.swVersion); stream.next(m.hwVersion); @@ -201,7 +194,7 @@ struct Serializer > { m.extension.clear(); int N = (count - 40) / 30; m.extension.reserve(N); - typename Msg::_extension_type::value_type ext; + typename ublox_msgs::MonVER_::_extension_type::value_type ext; for (int i = 0; i < N; i++) { // Read each extension string stream.next(ext); @@ -209,17 +202,18 @@ struct Serializer > { } } - static uint32_t serializedLength(typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::MonVER_ &m) { return 40 + (30 * m.extension.size()); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::MonVER_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.swVersion); stream.next(m.hwVersion); - for(std::size_t i = 0; i < m.extension.size(); ++i) + for (std::size_t i = 0; i < m.extension.size(); ++i) { ros::serialization::serialize(stream, m.extension[i]); + } } }; @@ -228,11 +222,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::NavDGPS_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::NavDGPS_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.age); @@ -242,17 +233,18 @@ struct Serializer > { stream.next(m.status); stream.next(m.reserved1); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::NavDGPS_ &m) { return 16 + 12 * m.numCh; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + const ublox_msgs::NavDGPS_ &m) { + if (m.sv.size() != m.numCh) { ROS_ERROR("NavDGPS numCh must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -260,11 +252,12 @@ struct Serializer > { stream.next(m.age); stream.next(m.baseId); stream.next(m.baseHealth); - stream.next(static_cast(m.sv.size())); + stream.next(static_cast::_numCh_type>(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -274,11 +267,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::NavSBAS_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::NavSBAS_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.geo); @@ -288,17 +278,18 @@ struct Serializer > { stream.next(m.cnt); stream.next(m.reserved0); m.sv.resize(m.cnt); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::NavSBAS_ &m) { return 12 + 12 * m.cnt; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.cnt) { + const ublox_msgs::NavSBAS_ &m) { + if (m.sv.size() != m.cnt) { ROS_ERROR("NavSBAS cnt must equal sv size"); } ros::serialization::OStream stream(data, size); @@ -307,10 +298,11 @@ struct Serializer > { stream.next(m.mode); stream.next(m.sys); stream.next(m.service); - stream.next(static_cast(m.sv.size())); + stream.next(static_cast::_cnt_type>(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -319,37 +311,36 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::NavSAT_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::NavSAT_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); stream.next(m.numSvs); stream.next(m.reserved0); m.sv.resize(m.numSvs); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::NavSAT_ &m) { return 8 + 12 * m.numSvs; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSvs) { + const ublox_msgs::NavSAT_ &m) { + if (m.sv.size() != m.numSvs) { ROS_ERROR("NavSAT numSvs must equal sv size"); } ros::serialization::OStream stream(data, size); stream.next(m.iTOW); stream.next(m.version); - stream.next(static_cast(m.sv.size())); + stream.next(static_cast::_numSvs_type>(m.sv.size())); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -358,37 +349,36 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::NavSVINFO_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::NavSVINFO_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.numCh); stream.next(m.globalFlags); stream.next(m.reserved2); m.sv.resize(m.numCh); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::NavSVINFO_ &m) { return 8 + 12 * m.numCh; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numCh) { + const ublox_msgs::NavSVINFO_ &m) { + if (m.sv.size() != m.numCh) { ROS_ERROR("NavSVINFO numCh must equal sv size"); } ros::serialization::OStream stream(data, size); stream.next(m.iTOW); - stream.next(static_cast(m.sv.size())); + stream.next(static_cast::_numCh_type>(m.sv.size())); stream.next(m.globalFlags); stream.next(m.reserved2); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -397,37 +387,36 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::RxmRAW_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmRAW_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.numSV); stream.next(m.reserved1); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmRAW_ &m) { return 8 + 24 * m.numSV; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + const ublox_msgs::RxmRAW_ &m) { + if (m.sv.size() != m.numSV) { ROS_ERROR("RxmRAW numSV must equal sv size"); } ros::serialization::OStream stream(data, size); stream.next(m.rcvTOW); stream.next(m.week); - stream.next(static_cast(m.sv.size())); + stream.next(static_cast::_numSV_type>(m.sv.size())); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -436,11 +425,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::RxmRAWX_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmRAWX_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.rcvTOW); stream.next(m.week); @@ -450,29 +436,31 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.meas.resize(m.numMeas); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) { ros::serialization::deserialize(stream, m.meas[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmRAWX_ &m) { return 16 + 32 * m.numMeas; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.meas.size() != m.numMeas) { + const ublox_msgs::RxmRAWX_ &m) { + if (m.meas.size() != m.numMeas) { ROS_ERROR("RxmRAWX numMeas must equal meas size"); } ros::serialization::OStream stream(data, size); stream.next(m.rcvTOW); stream.next(m.week); stream.next(m.leapS); - stream.next(static_cast(m.meas.size())); + stream.next(static_cast::_numMeas_type>(m.meas.size())); stream.next(m.recStat); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.meas.size(); ++i) + for (std::size_t i = 0; i < m.meas.size(); ++i) { ros::serialization::serialize(stream, m.meas[i]); + } } }; @@ -481,11 +469,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::RxmSFRBX_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmSFRBX_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.gnssId); stream.next(m.svId); @@ -496,17 +481,18 @@ struct Serializer > { stream.next(m.version); stream.next(m.reserved1); m.dwrd.resize(m.numWords); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { ros::serialization::deserialize(stream, m.dwrd[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmSFRBX_ &m) { return 8 + 4 * m.numWords; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.dwrd.size() != m.numWords) { + const ublox_msgs::RxmSFRBX_ &m) { + if (m.dwrd.size() != m.numWords) { ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); } ros::serialization::OStream stream(data, size); @@ -514,12 +500,13 @@ struct Serializer > { stream.next(m.svId); stream.next(m.reserved0); stream.next(m.freqId); - stream.next(static_cast(m.dwrd.size())); + stream.next(static_cast::_numWords_type>(m.dwrd.size())); stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { ros::serialization::serialize(stream, m.dwrd[i]); + } } }; @@ -528,37 +515,36 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::RxmSVSI_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmSVSI_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.week); stream.next(m.numVis); stream.next(m.numSV); m.sv.resize(m.numSV); - for(std::size_t i = 0; i < m.sv.size(); ++i) + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmSVSI_ &m) { return 8 + 6 * m.numSV; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sv.size() != m.numSV) { + const ublox_msgs::RxmSVSI_ &m) { + if (m.sv.size() != m.numSV) { ROS_ERROR("RxmSVSI numSV must equal sv size"); } ros::serialization::OStream stream(data, size); stream.next(m.iTOW); stream.next(m.week); stream.next(m.numVis); - stream.next(static_cast(m.sv.size())); - for(std::size_t i = 0; i < m.sv.size(); ++i) + stream.next(static_cast::_numSV_type>(m.sv.size())); + for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); + } } }; @@ -567,37 +553,35 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::RxmALM_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmALM_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); - if(count == 40) { - typename Msg::_dwrd_type::value_type temp; + if (count == 40) { + typename ublox_msgs::RxmALM_::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp); m.dwrd.push_back(temp); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmALM_ &m) { return 8 + (4 * m.dwrd.size()); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::RxmALM_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { ros::serialization::serialize(stream, m.dwrd[i]); + } } }; @@ -607,11 +591,8 @@ struct Serializer > { template struct Serializer > { - typedef ublox_msgs::RxmEPH_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::RxmEPH_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); @@ -620,43 +601,46 @@ struct Serializer > m.sf3d.clear(); if (count == 104) { - typename Msg::_sf1d_type::value_type temp1; - typename Msg::_sf2d_type::value_type temp2; - typename Msg::_sf3d_type::value_type temp3; + typename ublox_msgs::RxmEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::RxmEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::RxmEPH_::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::RxmEPH_ &m) { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::RxmEPH_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for(std::size_t i = 0; i < m.sf1d.size(); ++i) { ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + } + for(std::size_t i = 0; i < m.sf2d.size(); ++i) { ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + } + for(std::size_t i = 0; i < m.sf3d.size(); ++i) { ros::serialization::serialize(stream, m.sf3d[i]); + } } }; @@ -665,37 +649,35 @@ struct Serializer > /// template struct Serializer > { - typedef ublox_msgs::AidALM_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::AidALM_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); if (count == 40) { - typename Msg::_dwrd_type::value_type temp; + typename ublox_msgs::AidALM_::_dwrd_type::value_type temp; m.dwrd.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp); m.dwrd.push_back(temp); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::AidALM_ &m) { return 8 + (4 * m.dwrd.size()); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::AidALM_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.week); - for(std::size_t i = 0; i < m.dwrd.size(); ++i) + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { ros::serialization::serialize(stream, m.dwrd[i]); + } } }; @@ -705,11 +687,8 @@ struct Serializer > { template struct Serializer > { - typedef ublox_msgs::AidEPH_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::AidEPH_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); @@ -718,42 +697,45 @@ struct Serializer > m.sf3d.clear(); if (count == 104) { - typename Msg::_sf1d_type::value_type temp1; - typename Msg::_sf2d_type::value_type temp2; - typename Msg::_sf3d_type::value_type temp3; + typename ublox_msgs::AidEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::AidEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::AidEPH_::_sf3d_type::value_type temp3; m.sf1d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp1); m.sf1d.push_back(temp1); } m.sf2d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp2); m.sf2d.push_back(temp2); } m.sf3d.resize(8); - for(std::size_t i = 0; i < 8; ++i) { + for (std::size_t i = 0; i < 8; ++i) { stream.next(temp3); m.sf3d.push_back(temp3); } } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::AidEPH_ &m) { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::AidEPH_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) + for (std::size_t i = 0; i < m.sf1d.size(); ++i) { ros::serialization::serialize(stream, m.sf1d[i]); - for(std::size_t i = 0; i < m.sf2d.size(); ++i) + } + for (std::size_t i = 0; i < m.sf2d.size(); ++i) { ros::serialization::serialize(stream, m.sf2d[i]); - for(std::size_t i = 0; i < m.sf3d.size(); ++i) + } + for (std::size_t i = 0; i < m.sf3d.size(); ++i) { ros::serialization::serialize(stream, m.sf3d[i]); + } } }; @@ -763,11 +745,8 @@ struct Serializer > /// template struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::EsfMEAS_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.timeTag); stream.next(m.flags); @@ -777,29 +756,32 @@ struct Serializer > { int data_size = (count - (calib_valid ? 12 : 8)) / 4; // Repeating block m.data.resize(data_size); - for(std::size_t i = 0; i < data_size; ++i) + for (std::size_t i = 0; i < data_size; ++i) { ros::serialization::deserialize(stream, m.data[i]); + } // Optional block - if(calib_valid) { + if (calib_valid) { m.calibTtag.resize(1); ros::serialization::deserialize(stream, m.calibTtag[0]); } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::EsfMEAS_ &m) { return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::EsfMEAS_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.timeTag); stream.next(m.flags); stream.next(m.id); - for(std::size_t i = 0; i < m.data.size(); ++i) + for (std::size_t i = 0; i < m.data.size(); ++i) { ros::serialization::serialize(stream, m.data[i]); - for(std::size_t i = 0; i < m.calibTtag.size(); ++i) + } + for (std::size_t i = 0; i < m.calibTtag.size(); ++i) { ros::serialization::serialize(stream, m.calibTtag[i]); + } } }; @@ -808,31 +790,29 @@ struct Serializer > { /// template struct Serializer > { - typedef boost::call_traits > - CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::EsfRAW_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.reserved0); m.blocks.clear(); int num_blocks = (count - 4) / 8; m.blocks.resize(num_blocks); - for(std::size_t i = 0; i < num_blocks; ++i) + for (std::size_t i = 0; i < num_blocks; ++i) { ros::serialization::deserialize(stream, m.blocks[i]); + } } - - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::EsfRAW_ &m) { return 4 + 8 * m.blocks.size(); } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { + const ublox_msgs::EsfRAW_ &m) { ros::serialization::OStream stream(data, size); stream.next(m.reserved0); - for(std::size_t i = 0; i < m.blocks.size(); ++i) + for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::serialize(stream, m.blocks[i]); + } } }; @@ -841,11 +821,8 @@ struct Serializer > { /// template struct Serializer > { - typedef ublox_msgs::EsfSTATUS_ Msg; - typedef boost::call_traits CallTraits; - static void read(const uint8_t *data, uint32_t count, - typename CallTraits::reference m) { + ublox_msgs::EsfSTATUS_ &m) { ros::serialization::IStream stream(const_cast(data), count); stream.next(m.iTOW); stream.next(m.version); @@ -853,17 +830,18 @@ struct Serializer > { stream.next(m.reserved2); stream.next(m.numSens); m.sens.resize(m.numSens); - for(std::size_t i = 0; i < m.sens.size(); ++i) + for (std::size_t i = 0; i < m.sens.size(); ++i) { ros::serialization::deserialize(stream, m.sens[i]); + } } - static uint32_t serializedLength (typename CallTraits::param_type m) { + static uint32_t serializedLength(const ublox_msgs::EsfSTATUS_ &m) { return 16 + 4 * m.numSens; } static void write(uint8_t *data, uint32_t size, - typename CallTraits::param_type m) { - if(m.sens.size() != m.numSens) { + const ublox_msgs::EsfSTATUS_ &m) { + if (m.sens.size() != m.numSens) { ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); } ros::serialization::OStream stream(data, size); @@ -871,9 +849,10 @@ struct Serializer > { stream.next(m.version); stream.next(m.fusionMode); stream.next(m.reserved2); - stream.next(static_cast(m.sens.size())); - for(std::size_t i = 0; i < m.sens.size(); ++i) + stream.next(static_cast::_numSens_type>(m.sens.size())); + for (std::size_t i = 0; i < m.sens.size(); ++i) { ros::serialization::serialize(stream, m.sens[i]); + } } }; From 76b74562963cc6b7c0708c57ff70b47a017448a5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 11:08:46 -0500 Subject: [PATCH 005/152] Rearrange messages. This is mostly to make the messages compatible with ROS 2, which is more strict on what it accepts. There should be no functional change here. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 2 +- ublox_gps/include/ublox_gps/node.hpp | 56 ++-- ublox_gps/src/gps.cpp | 221 +++++++------ ublox_gps/src/node.cpp | 300 +++++++++--------- .../ublox/serialization/ublox_msgs.hpp | 244 +++++++------- ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 10 +- ublox_msgs/msg/Ack.msg | 4 +- ublox_msgs/msg/AidALM.msg | 20 +- ublox_msgs/msg/AidEPH.msg | 8 +- ublox_msgs/msg/AidHUI.msg | 50 +-- ublox_msgs/msg/CfgANT.msg | 24 +- ublox_msgs/msg/CfgCFG.msg | 28 +- ublox_msgs/msg/CfgDAT.msg | 44 +-- ublox_msgs/msg/CfgDGNSS.msg | 6 +- ublox_msgs/msg/CfgGNSS.msg | 38 +-- .../{CfgGNSS_Block.msg => CfgGNSSBlock.msg} | 32 +- ublox_msgs/msg/CfgHNR.msg | 8 +- ublox_msgs/msg/CfgINF.msg | 14 +- ublox_msgs/msg/CfgINFBlock.msg | 21 ++ ublox_msgs/msg/CfgINF_Block.msg | 21 -- ublox_msgs/msg/CfgMSG.msg | 10 +- ublox_msgs/msg/CfgNAV5.msg | 66 ++-- ublox_msgs/msg/CfgNAVX5.msg | 74 ++--- ublox_msgs/msg/CfgNMEA.msg | 48 +-- ublox_msgs/msg/CfgNMEA6.msg | 12 +- ublox_msgs/msg/CfgNMEA7.msg | 42 +-- ublox_msgs/msg/CfgPRT.msg | 40 +-- ublox_msgs/msg/CfgRATE.msg | 22 +- ublox_msgs/msg/CfgRST.msg | 10 +- ublox_msgs/msg/CfgSBAS.msg | 8 +- ublox_msgs/msg/CfgTMODE3.msg | 26 +- ublox_msgs/msg/CfgUSB.msg | 20 +- ublox_msgs/msg/EsfINS.msg | 34 +- ublox_msgs/msg/EsfMEAS.msg | 60 ++-- ublox_msgs/msg/EsfRAW.msg | 4 +- .../msg/{EsfRAW_Block.msg => EsfRAWBlock.msg} | 4 +- ublox_msgs/msg/EsfSTATUS.msg | 20 +- ublox_msgs/msg/EsfSTATUSSens.msg | 7 + ublox_msgs/msg/HnrPVT.msg | 32 +- ublox_msgs/msg/MgaGAL.msg | 44 +-- ublox_msgs/msg/MonGNSS.msg | 12 +- ublox_msgs/msg/MonHW.msg | 36 +-- ublox_msgs/msg/MonHW6.msg | 38 +-- ublox_msgs/msg/MonVER.msg | 6 +- ...nVER_Extension.msg => MonVERExtension.msg} | 0 ublox_msgs/msg/NavATT.msg | 8 +- ublox_msgs/msg/NavCLOCK.msg | 10 +- ublox_msgs/msg/NavDGPS.msg | 10 +- .../msg/{NavDGPS_SV.msg => NavDGPSSV.msg} | 2 +- ublox_msgs/msg/NavDOP.msg | 18 +- ublox_msgs/msg/NavPOSECEF.msg | 10 +- ublox_msgs/msg/NavPOSLLH.msg | 10 +- ublox_msgs/msg/NavPVT.msg | 56 ++-- ublox_msgs/msg/NavPVT7.msg | 52 +-- ublox_msgs/msg/NavRELPOSNED.msg | 38 +-- ublox_msgs/msg/NavRELPOSNED9.msg | 34 +- ublox_msgs/msg/NavSAT.msg | 12 +- .../msg/{NavSAT_SV.msg => NavSATSV.msg} | 74 ++--- ublox_msgs/msg/NavSBAS.msg | 6 +- .../msg/{NavSBAS_SV.msg => NavSBASSV.msg} | 4 +- ublox_msgs/msg/NavSOL.msg | 30 +- ublox_msgs/msg/NavSTATUS.msg | 26 +- ublox_msgs/msg/NavSVIN.msg | 26 +- ublox_msgs/msg/NavSVINFO.msg | 8 +- .../msg/{NavSVINFO_SV.msg => NavSVINFOSV.msg} | 18 +- ublox_msgs/msg/NavTIMEGPS.msg | 8 +- ublox_msgs/msg/NavTIMEUTC.msg | 12 +- ublox_msgs/msg/NavVELECEF.msg | 10 +- ublox_msgs/msg/NavVELNED.msg | 14 +- ublox_msgs/msg/RxmRAW.msg | 14 +- .../msg/{RxmRAW_SV.msg => RxmRAWSV.msg} | 10 +- ublox_msgs/msg/RxmRAWX.msg | 24 +- .../msg/{RxmRAWX_Meas.msg => RxmRAWXMeas.msg} | 24 +- ublox_msgs/msg/RxmRTCM.msg | 8 +- ublox_msgs/msg/RxmSFRBX.msg | 12 +- ublox_msgs/msg/RxmSVSI.msg | 8 +- ublox_msgs/msg/RxmSVSISV.msg | 22 ++ ublox_msgs/msg/TimTM2.msg | 22 +- ublox_msgs/msg/UpdSOS.msg | 20 +- .../msg/{UpdSOS_Ack.msg => UpdSOSAck.msg} | 24 +- ublox_msgs/src/ublox_msgs.cpp | 4 +- 81 files changed, 1289 insertions(+), 1225 deletions(-) rename ublox_msgs/msg/{CfgGNSS_Block.msg => CfgGNSSBlock.msg} (56%) create mode 100644 ublox_msgs/msg/CfgINFBlock.msg delete mode 100644 ublox_msgs/msg/CfgINF_Block.msg rename ublox_msgs/msg/{EsfRAW_Block.msg => EsfRAWBlock.msg} (72%) create mode 100644 ublox_msgs/msg/EsfSTATUSSens.msg rename ublox_msgs/msg/{MonVER_Extension.msg => MonVERExtension.msg} (100%) rename ublox_msgs/msg/{NavDGPS_SV.msg => NavDGPSSV.msg} (87%) rename ublox_msgs/msg/{NavSAT_SV.msg => NavSATSV.msg} (87%) rename ublox_msgs/msg/{NavSBAS_SV.msg => NavSBASSV.msg} (72%) rename ublox_msgs/msg/{NavSVINFO_SV.msg => NavSVINFOSV.msg} (93%) rename ublox_msgs/msg/{RxmRAW_SV.msg => RxmRAWSV.msg} (61%) rename ublox_msgs/msg/{RxmRAWX_Meas.msg => RxmRAWXMeas.msg} (63%) create mode 100644 ublox_msgs/msg/RxmSVSISV.msg rename ublox_msgs/msg/{UpdSOS_Ack.msg => UpdSOSAck.msg} (90%) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index d3881282..f6d3b458 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -448,7 +448,7 @@ class Gps { * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process */ - void processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m); + void processUpdSosAck(const ublox_msgs::UpdSOSAck &m); /** * @brief Execute save on shutdown procedure. diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 93d81a03..e5fcec40 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -766,7 +766,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { * @param m the message to publish */ void callbackNavPvt(const NavPVT& m) { - if(enabled["nav_pvt"]) { + if (enabled["nav_pvt"]) { // NavPVT publisher static ros::Publisher publisher = nh->advertise("navpvt", kROSQueueSize); @@ -807,23 +807,23 @@ class UbloxFirmware7Plus : public UbloxFirmware { fix.altitude = m.height * 1e-3; // to [m] // Set the Fix status bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; - if (fixOk && m.fixType >= m.FIX_TYPE_2D) { + if (fixOk && m.fix_type >= m.FIX_TYPE_2D) { fix.status.status = fix.status.STATUS_FIX; - if(m.flags & m.CARRIER_PHASE_FIXED) + if (m.flags & m.CARRIER_PHASE_FIXED) { fix.status.status = fix.status.STATUS_GBAS_FIX; - } - else { + } + } else { fix.status.status = fix.status.STATUS_NO_FIX; } // Set the service based on GNSS configuration fix.status.service = fix_status_service; // Set the position covariance - const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2] - const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2] - fix.position_covariance[0] = varH; - fix.position_covariance[4] = varH; - fix.position_covariance[8] = varV; + const double var_h = pow(m.h_acc / 1000.0, 2); // to [m^2] + const double var_v = pow(m.v_acc / 1000.0, 2); // to [m^2] + fix.position_covariance[0] = var_h; + fix.position_covariance[4] = var_h; + fix.position_covariance[8] = var_v; fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; @@ -840,15 +840,15 @@ class UbloxFirmware7Plus : public UbloxFirmware { velocity.header.frame_id = frame_id; // convert to XYZ linear velocity [m/s] in ENU - velocity.twist.twist.linear.x = m.velE * 1e-3; - velocity.twist.twist.linear.y = m.velN * 1e-3; - velocity.twist.twist.linear.z = -m.velD * 1e-3; + velocity.twist.twist.linear.x = m.vel_e * 1e-3; + velocity.twist.twist.linear.y = m.vel_n * 1e-3; + velocity.twist.twist.linear.z = -m.vel_d * 1e-3; // Set the covariance - const double covSpeed = pow(m.sAcc * 1e-3, 2); + const double cov_speed = pow(m.s_acc * 1e-3, 2); const int cols = 6; - velocity.twist.covariance[cols * 0 + 0] = covSpeed; - velocity.twist.covariance[cols * 1 + 1] = covSpeed; - velocity.twist.covariance[cols * 2 + 2] = covSpeed; + velocity.twist.covariance[cols * 0 + 0] = cov_speed; + velocity.twist.covariance[cols * 1 + 1] = cov_speed; + velocity.twist.covariance[cols * 2 + 2] = cov_speed; velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported velocityPublisher.publish(velocity); @@ -868,21 +868,21 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { // check the last message, convert to diagnostic - if (last_nav_pvt_.fixType == + if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) { + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_2D) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "2D fix"; - } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) { + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_3D) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_pvt_.fixType == + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_pvt_.fixType == + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time only fix"; @@ -894,20 +894,20 @@ class UbloxFirmware7Plus : public UbloxFirmware { stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { + if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } // append last fix position - stat.add("iTOW [ms]", last_nav_pvt_.iTOW); + stat.add("iTOW [ms]", last_nav_pvt_.i_tow); stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7); stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7); stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pvt_.hMSL * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pvt_.hAcc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pvt_.vAcc * 1e-3); - stat.add("# SVs used", (int)last_nav_pvt_.numSV); + stat.add("Height above MSL [m]", last_nav_pvt_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pvt_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pvt_.v_acc * 1e-3); + stat.add("# SVs used", (int)last_nav_pvt_.num_sv); } //! The last received NavPVT message diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index dad5d19d..a42a84de 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -42,13 +42,15 @@ const std::chrono::milliseconds Gps::default_timeout_ = static_cast(Gps::kDefaultAckTimeout * 1000)); Gps::Gps() : configured_(false), config_on_startup_flag_(true) { - subscribeAcks(); + subscribeAcks(); } Gps::~Gps() { close(); } void Gps::setWorker(const boost::shared_ptr& worker) { - if (worker_) return; + if (worker_) { + return; + } worker_ = worker; worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, &callbacks_, _1, _2)); @@ -63,7 +65,7 @@ void Gps::subscribeAcks() { subscribeId(boost::bind(&Gps::processAck, this, _1), ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( + subscribe( boost::bind(&Gps::processUpdSosAck, this, _1)); } @@ -71,27 +73,27 @@ void Gps::processAck(const ublox_msgs::Ack &m) { // Process ACK/NACK messages Ack ack; ack.type = ACK; - ack.class_id = m.clsID; - ack.msg_id = m.msgID; + ack.class_id = m.cls_id; + ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.clsID, m.msgID); + m.cls_id, m.msg_id); } void Gps::processNack(const ublox_msgs::Ack &m) { // Process ACK/NACK messages Ack ack; ack.type = NACK; - ack.class_id = m.clsID; - ack.msg_id = m.msgID; + ack.class_id = m.cls_id; + ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, boost::memory_order_seq_cst); - ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.clsID, m.msgID); + ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } -void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { - if (m.cmd == UpdSOS_Ack::CMD_BACKUP_CREATE_ACK) { +void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { + if (m.cmd == UpdSOSAck::CMD_BACKUP_CREATE_ACK) { Ack ack; ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; ack.class_id = m.CLASS_ID; @@ -100,8 +102,9 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOS_Ack &m) { ack_.store(ack, boost::memory_order_seq_cst); ROS_DEBUG_COND(ack.type == ACK && debug >= 2, "U-blox: received UPD SOS Backup ACK"); - if(ack.type == NACK) + if (ack.type == NACK) { ROS_ERROR("U-blox: received UPD SOS Backup NACK"); + } } } @@ -123,8 +126,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - if(BOOST_VERSION < 106600) - { + if (BOOST_VERSION < 106600) { // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but // until v1.66.0 there was a bug which didn't enable the relevant code, // fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 @@ -136,7 +138,9 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } // Set the I/O worker - if (worker_) return; + if (worker_) { + return; + } setWorker(boost::shared_ptr( new AsyncWorker(serial, io_service))); @@ -147,11 +151,13 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { - if (current_baudrate.value() == baudrate) + if (current_baudrate.value() == baudrate) { break; + } // Don't step down, unless the desired baudrate is lower - if(current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) + if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) { continue; + } serial->set_option( boost::asio::serial_port_base::baud_rate(kBaudrates[i])); std::this_thread::sleep_for( @@ -161,7 +167,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } if (config_on_startup_flag_) { configured_ = configUart1(baudrate, uart_in, uart_out); - if(!configured_ || current_baudrate.value() != baudrate) { + if (!configured_ || current_baudrate.value() != baudrate) { throw std::runtime_error("Could not configure serial baud rate"); } } else { @@ -186,7 +192,9 @@ void Gps::resetSerial(std::string port) { ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker - if (worker_) return; + if (worker_) { + return; + } setWorker(boost::shared_ptr( new AsyncWorker(serial, io_service))); configured_ = false; @@ -198,15 +206,15 @@ void Gps::resetSerial(std::string port) { ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } - CfgPRT prt; - if(!read(prt, default_timeout_)) { + ublox_msgs::CfgPRT prt; + if (!read(prt, default_timeout_)) { ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", "message"); return; } // Set the baudrate - serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baudRate)); + serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baud_rate)); configured_ = true; } @@ -240,18 +248,21 @@ void Gps::initializeTcp(std::string host, std::string port) { ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str()); - if (worker_) return; + if (worker_) { + return; + } setWorker(boost::shared_ptr( new AsyncWorker(socket, io_service))); } void Gps::close() { - if(save_on_shutdown_) { - if(saveOnShutdown()) + if (save_on_shutdown_) { + if (saveOnShutdown()) { ROS_INFO("U-Blox Flash BBR saved"); - else + } else { ROS_INFO("U-Blox Flash BBR failed to save"); + } } worker_.reset(); configured_ = false; @@ -262,23 +273,25 @@ void Gps::reset(const std::chrono::milliseconds& wait) { configured_ = false; // sleep because of undefined behavior after I/O reset std::this_thread::sleep_for(wait); - if (host_ == "") + if (host_ == "") { resetSerial(port_); - else + } else { initializeTcp(host_, port_); + } } bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { ROS_WARN("Resetting u-blox. If device address changes, %s", "node must be relaunched."); - CfgRST rst; - rst.navBbrMask = nav_bbr_mask; - rst.resetMode = reset_mode; + ublox_msgs::CfgRST rst; + rst.nav_bbr_mask = nav_bbr_mask; + rst.reset_mode = reset_mode; // Don't wait for ACK, return if it fails - if (!configure(rst, false)) + if (!configure(rst, false)) { return false; + } return true; } @@ -286,12 +299,14 @@ bool Gps::configGnss(CfgGNSS gnss, const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox ROS_DEBUG("Re-configuring GNSS."); - if (!configure(gnss)) + if (!configure(gnss)) { return false; + } // Cold reset the GNSS ROS_WARN("GNSS re-configured, cold resetting device."); - if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS)) + if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS)) { return false; + } ros::Duration(1.0).sleep(); // Reset the I/O reset(wait); @@ -300,39 +315,42 @@ bool Gps::configGnss(CfgGNSS gnss, bool Gps::saveOnShutdown() { // Command the receiver to stop - CfgRST rst; - rst.navBbrMask = rst.NAV_BBR_HOT_START; - rst.resetMode = rst.RESET_MODE_GNSS_STOP; - if(!configure(rst)) + ublox_msgs::CfgRST rst; + rst.nav_bbr_mask = rst.NAV_BBR_HOT_START; + rst.reset_mode = rst.RESET_MODE_GNSS_STOP; + if (!configure(rst)) { return false; + } // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - UpdSOS backup; + ublox_msgs::UpdSOS backup; return configure(backup); } bool Gps::clearBbr() { // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - UpdSOS sos; + ublox_msgs::UpdSOS sos; sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR; return configure(sos); } bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; + if (!worker_) { + return true; + } ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", baudrate, in_proto_mask, out_proto_mask); - CfgPRT port; - port.portID = CfgPRT::PORT_ID_UART1; - port.baudRate = baudrate; + ublox_msgs::CfgPRT port; + port.port_id = CfgPRT::PORT_ID_UART1; + port.baud_rate = baudrate; port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; - port.inProtoMask = in_proto_mask; - port.outProtoMask = out_proto_mask; + port.in_proto_mask = in_proto_mask; + port.out_proto_mask = out_proto_mask; return configure(port); } @@ -346,18 +364,18 @@ bool Gps::disableUart1(CfgPRT& prev_config) { ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } - if(!read(prev_config, default_timeout_)) { + if (!read(prev_config, default_timeout_)) { ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); return false; } // Keep original settings, but disable in/out - CfgPRT port; - port.portID = CfgPRT::PORT_ID_UART1; + ublox_msgs::CfgPRT port; + port.port_id = CfgPRT::PORT_ID_UART1; port.mode = prev_config.mode; - port.baudRate = prev_config.baudRate; - port.inProtoMask = 0; - port.outProtoMask = 0; - port.txReady = prev_config.txReady; + port.baud_rate = prev_config.baud_rate; + port.in_proto_mask = 0; + port.out_proto_mask = 0; + port.tx_ready = prev_config.tx_ready; port.flags = prev_config.flags; return configure(port); } @@ -365,16 +383,18 @@ bool Gps::disableUart1(CfgPRT& prev_config) { bool Gps::configUsb(uint16_t tx_ready, uint16_t in_proto_mask, uint16_t out_proto_mask) { - if (!worker_) return true; + if (!worker_) { + return true; + } ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", tx_ready, in_proto_mask, out_proto_mask); - CfgPRT port; - port.portID = CfgPRT::PORT_ID_USB; - port.txReady = tx_ready; - port.inProtoMask = in_proto_mask; - port.outProtoMask = out_proto_mask; + ublox_msgs::CfgPRT port; + port.port_id = CfgPRT::PORT_ID_USB; + port.tx_ready = tx_ready; + port.in_proto_mask = in_proto_mask; + port.out_proto_mask = out_proto_mask; return configure(port); } @@ -382,17 +402,17 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", meas_rate, nav_rate); - CfgRATE rate; - rate.measRate = meas_rate; - rate.navRate = nav_rate; // must be fixed at 1 for ublox 5 and 6 - rate.timeRef = CfgRATE::TIME_REF_GPS; + ublox_msgs::CfgRATE rate; + rate.meas_rate = meas_rate; + rate.nav_rate = nav_rate; // must be fixed at 1 for ublox 5 and 6 + rate.time_ref = CfgRATE::TIME_REF_GPS; return configure(rate); } bool Gps::configRtcm(std::vector ids, std::vector rates) { - for(size_t i = 0; i < ids.size(); ++i) { + for (size_t i = 0; i < ids.size(); ++i) { ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if(!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { + if (!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); return false; } @@ -406,7 +426,7 @@ bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { ublox_msgs::CfgSBAS msg; msg.mode = (enable ? CfgSBAS::MODE_ENABLED : 0); msg.usage = usage; - msg.maxSBAS = max_sbas; + msg.max_sbas = max_sbas; return configure(msg); } @@ -414,7 +434,7 @@ bool Gps::configTmode3Fixed(bool lla_flag, std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc) { - if(arp_position.size() != 3 || arp_position_hp.size() != 3) { + if (arp_position.size() != 3 || arp_position_hp.size() != 3) { ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", "& arp_position_hp args must be 3"); return false; @@ -422,45 +442,45 @@ bool Gps::configTmode3Fixed(bool lla_flag, ROS_DEBUG("Configuring TMODE3 to Fixed"); - CfgTMODE3 tmode3; + ublox_msgs::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK; tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; // Set position - if(lla_flag) { + if (lla_flag) { // Convert from [deg] to [deg * 1e-7] - tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e7); - tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e7); - tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e7); + tmode3.ecef_x_or_lat = (int)round(arp_position[0] * 1e7); + tmode3.ecef_y_or_lon = (int)round(arp_position[1] * 1e7); + tmode3.ecef_z_or_alt = (int)round(arp_position[2] * 1e7); } else { // Convert from m to cm - tmode3.ecefXOrLat = (int)round(arp_position[0] * 1e2); - tmode3.ecefYOrLon = (int)round(arp_position[1] * 1e2); - tmode3.ecefZOrAlt = (int)round(arp_position[2] * 1e2); + tmode3.ecef_x_or_lat = (int)round(arp_position[0] * 1e2); + tmode3.ecef_y_or_lon = (int)round(arp_position[1] * 1e2); + tmode3.ecef_z_or_alt = (int)round(arp_position[2] * 1e2); } - tmode3.ecefXOrLatHP = arp_position_hp[0]; - tmode3.ecefYOrLonHP = arp_position_hp[1]; - tmode3.ecefZOrAltHP = arp_position_hp[2]; + tmode3.ecef_x_or_lat_hp = arp_position_hp[0]; + tmode3.ecef_y_or_lon_hp = arp_position_hp[1]; + tmode3.ecef_z_or_alt_hp = arp_position_hp[2]; // Convert from m to [0.1 mm] - tmode3.fixedPosAcc = (uint32_t)round(fixed_pos_acc * 1e4); + tmode3.fixed_pos_acc = (uint32_t)round(fixed_pos_acc * 1e4); return configure(tmode3); } bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) { - CfgTMODE3 tmode3; + ublox_msgs::CfgTMODE3 tmode3; ROS_DEBUG("Setting TMODE3 to Survey In"); tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; - tmode3.svinMinDur = svin_min_dur; + tmode3.svin_min_dur = svin_min_dur; // Convert from m to [0.1 mm] - tmode3.svinAccLimit = (int)round(svin_acc_limit * 1e4); + tmode3.svin_acc_limit = (int)round(svin_acc_limit * 1e4); return configure(tmode3); } bool Gps::disableTmode3() { ROS_DEBUG("Disabling TMODE3"); - CfgTMODE3 tmode3; + ublox_msgs::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK; return configure(tmode3); } @@ -469,8 +489,8 @@ bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, message_id, rate); ublox_msgs::CfgMSG msg; - msg.msgClass = class_id; - msg.msgID = message_id; + msg.msg_class = class_id; + msg.msg_id = message_id; msg.rate = rate; return configure(msg); } @@ -479,7 +499,7 @@ bool Gps::setDynamicModel(uint8_t model) { ROS_DEBUG("Setting dynamic model to %u", model); ublox_msgs::CfgNAV5 msg; - msg.dynModel = model; + msg.dyn_model = model; msg.mask = ublox_msgs::CfgNAV5::MASK_DYN; return configure(msg); } @@ -488,7 +508,7 @@ bool Gps::setFixMode(uint8_t mode) { ROS_DEBUG("Setting fix mode to %u", mode); ublox_msgs::CfgNAV5 msg; - msg.fixMode = mode; + msg.fix_mode = mode; msg.mask = ublox_msgs::CfgNAV5::MASK_FIX_MODE; return configure(msg); } @@ -497,7 +517,7 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { ROS_DEBUG("Setting DR Limit to %u", limit); ublox_msgs::CfgNAV5 msg; - msg.drLimit = limit; + msg.dr_limit = limit; msg.mask = ublox_msgs::CfgNAV5::MASK_DR_LIM; return configure(msg); } @@ -506,15 +526,15 @@ bool Gps::setPpp(bool enable) { ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; - msg.usePPP = enable; + msg.use_ppp = enable; msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; return configure(msg); } bool Gps::setDgnss(uint8_t mode) { - CfgDGNSS cfg; + ublox_msgs::CfgDGNSS cfg; ROS_DEBUG("Setting DGNSS mode to %u", mode); - cfg.dgnssMode = mode; + cfg.dgnss_mode = mode; return configure(cfg); } @@ -522,19 +542,22 @@ bool Gps::setUseAdr(bool enable) { ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::CfgNAVX5 msg; - msg.useAdr = enable; + msg.use_adr = enable; msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR; return configure(msg); } bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector& payload) { - if (!worker_) return false; + if (!worker_) { + return false; + } std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); - if (!writer.write(payload.data(), payload.size(), class_id, message_id)) + if (!writer.write(payload.data(), payload.size(), class_id, message_id)) { return false; + } worker_->send(out.data(), writer.end() - out.data()); return true; @@ -562,7 +585,9 @@ bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, } void Gps::setRawDataCallback(const Worker::Callback& callback) { - if (! worker_) return; + if (!worker_) { + return; + } worker_->setRawDataCallback(callback); } @@ -570,15 +595,15 @@ bool Gps::setUTCtime() { ROS_DEBUG("Setting time to UTC time"); ublox_msgs::CfgNAV5 msg; - msg.utcStandard = 3; + msg.utc_standard = 3; return configure(msg); } bool Gps::setTimtm2(uint8_t rate) { ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate ); ublox_msgs::CfgMSG msg; - msg.msgClass = ublox_msgs::TimTM2::CLASS_ID; - msg.msgID = ublox_msgs::TimTM2::MESSAGE_ID; + msg.msg_class = ublox_msgs::TimTM2::CLASS_ID; + msg.msg_id = ublox_msgs::TimTM2::MESSAGE_ID; msg.rate = rate; return configure(msg); } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 523f9674..61a3333d 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -132,10 +132,10 @@ void UbloxNode::getRosParams() { nh->param("frame_id", frame_id, std::string("gps")); // Save configuration parameters - getRosUint("load/mask", load_.loadMask, 0); - getRosUint("load/device", load_.deviceMask, 0); - getRosUint("save/mask", save_.saveMask, 0); - getRosUint("save/device", save_.deviceMask, 0); + getRosUint("load/mask", load_.load_mask, 0); + getRosUint("load/device", load_.device_mask, 0); + getRosUint("save/mask", save_.save_mask, 0); + getRosUint("save/device", save_.device_mask, 0); // UART 1 params getRosUint("uart1/baudrate", baudrate_, 9600); @@ -188,7 +188,7 @@ void UbloxNode::getRosParams() { nh->param("dat/set", set_dat_, false); if(set_dat_) { std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.majA) + if (!nh->getParam("dat/majA", cfg_dat_.maj_a) || nh->getParam("dat/flat", cfg_dat_.flat) || nh->getParam("dat/shift", shift) || nh->getParam("dat/rot", rot) @@ -198,18 +198,18 @@ void UbloxNode::getRosParams() { if(shift.size() != 3 || rot.size() != 3) throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + "must be 3"); - checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA"); + checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat/majA"); checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); checkRange(shift, 0.0, 500.0, "dat/shift"); - cfg_dat_.dX = shift[0]; - cfg_dat_.dY = shift[1]; - cfg_dat_.dZ = shift[2]; + cfg_dat_.d_x = shift[0]; + cfg_dat_.d_y = shift[1]; + cfg_dat_.d_z = shift[2]; checkRange(rot, -5000.0, 5000.0, "dat/rot"); - cfg_dat_.rotX = rot[0]; - cfg_dat_.rotY = rot[1]; - cfg_dat_.rotZ = rot[2]; + cfg_dat_.rot_x = rot[0]; + cfg_dat_.rot_y = rot[1]; + cfg_dat_.rot_z = rot[2]; checkRange(cfg_dat_.scale, 0.0, 50.0, "scale"); } @@ -352,8 +352,8 @@ void UbloxNode::processMonVer() { if (!gps.poll(monVer)) throw std::runtime_error("Failed to poll MonVER & set relevant settings"); - ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(), - monVer.hwVersion.c_array()); + ROS_DEBUG("%s, HW VER: %s", monVer.sw_version.c_array(), + monVer.hw_version.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); @@ -416,12 +416,12 @@ bool UbloxNode::configureUblox() { try { if (!gps.isInitialized()) throw std::runtime_error("Failed to initialize."); - if (load_.loadMask != 0) { - ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.loadMask); + if (load_.load_mask != 0) { + ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.load_mask); if (!gps.configure(load_)) throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); - if (load_.loadMask & load_.MASK_IO_PORT) { + if (load_.load_mask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); @@ -471,9 +471,9 @@ bool UbloxNode::configureUblox() { return false; } } - if (save_.saveMask != 0) { + if (save_.save_mask != 0) { ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", - save_.saveMask, save_.deviceMask); + save_.save_mask, save_.device_mask); if(!gps.configure(save_)) ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } @@ -487,32 +487,35 @@ bool UbloxNode::configureUblox() { void UbloxNode::configureInf() { ublox_msgs::CfgINF msg; // Subscribe to UBX INF messages - ublox_msgs::CfgINF_Block block; - block.protocolID = block.PROTOCOL_ID_UBX; + ublox_msgs::CfgINFBlock block; + block.protocol_id = block.PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) | (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) | (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); - for (int i = 0; i < block.infMsgMask.size(); i++) - block.infMsgMask[i] = mask; + for (int i = 0; i < block.inf_msg_mask.size(); i++) { + block.inf_msg_mask[i] = mask; + } msg.blocks.push_back(block); // IF NMEA is enabled if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) { - ublox_msgs::CfgINF_Block block; - block.protocolID = block.PROTOCOL_ID_NMEA; + ublox_msgs::CfgINFBlock block; + block.protocol_id = block.PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port - for (int i = 0; i < block.infMsgMask.size(); i++) - block.infMsgMask[i] = mask; + for (int i = 0; i < block.inf_msg_mask.size(); i++) { + block.inf_msg_mask[i] = mask; + } msg.blocks.push_back(block); } ROS_DEBUG("Configuring INF messages"); - if (!gps.configure(msg)) + if (!gps.configure(msg)) { ROS_WARN("Failed to configure INF messages"); + } } void UbloxNode::initializeIo() { @@ -607,7 +610,7 @@ void UbloxFirmware6::getRosParams() { if (!getRosUint("nmea/version", cfg_nmea_.version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); if (!nh->getParam("nmea/compat", compat)) @@ -681,20 +684,20 @@ void UbloxFirmware6::subscribe() { void UbloxFirmware6::fixDiagnostic( diagnostic_updater::DiagnosticStatusWrapper& stat) { // Set the diagnostic level based on the fix status - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { + if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { stat.level = diagnostic_msgs::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) { + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_2D_FIX) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "2D fix"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) { + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_3D_FIX) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "3D fix"; - } else if (last_nav_sol_.gpsFix == + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time fix only"; } @@ -704,24 +707,24 @@ void UbloxFirmware6::fixDiagnostic( stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) { + if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_NO_FIX) { stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; stat.message = "No fix"; } // Add last fix position - stat.add("iTOW [ms]", last_nav_pos_.iTOW); + stat.add("iTOW [ms]", last_nav_pos_.i_tow); stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7); stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7); stat.add("Altitude [m]", last_nav_pos_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pos_.hMSL * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pos_.hAcc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pos_.vAcc * 1e-3); - stat.add("# SVs used", (int)last_nav_sol_.numSV); + stat.add("Height above MSL [m]", last_nav_pos_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pos_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pos_.v_acc * 1e-3); + stat.add("# SVs used", (int)last_nav_sol_.num_sv); } void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if(enabled["nav_posllh"]) { + if (enabled["nav_posllh"]) { static ros::Publisher publisher = nh->advertise("navposllh", kROSQueueSize); publisher.publish(m); @@ -730,28 +733,29 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { // Position message static ros::Publisher fixPublisher = nh->advertise("fix", kROSQueueSize); - if (m.iTOW == last_nav_vel_.iTOW) + if (m.i_tow == last_nav_vel_.i_tow) { fix_.header.stamp = velocity_.header.stamp; // use last timestamp - else + } else { fix_.header.stamp = ros::Time::now(); // new timestamp + } fix_.header.frame_id = frame_id; fix_.latitude = m.lat * 1e-7; fix_.longitude = m.lon * 1e-7; fix_.altitude = m.height * 1e-3; - if (last_nav_sol_.gpsFix >= last_nav_sol_.GPS_2D_FIX) + if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) fix_.status.status = fix_.status.STATUS_FIX; else fix_.status.status = fix_.status.STATUS_NO_FIX; // Convert from mm to m - const double varH = pow(m.hAcc / 1000.0, 2); - const double varV = pow(m.vAcc / 1000.0, 2); + const double var_h = pow(m.h_acc / 1000.0, 2); + const double var_v = pow(m.v_acc / 1000.0, 2); - fix_.position_covariance[0] = varH; - fix_.position_covariance[4] = varH; - fix_.position_covariance[8] = varV; + fix_.position_covariance[0] = var_h; + fix_.position_covariance[4] = var_h; + fix_.position_covariance[8] = var_v; fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; @@ -764,7 +768,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if(enabled["nav_velned"]) { + if (enabled["nav_velned"]) { static ros::Publisher publisher = nh->advertise("navvelned", kROSQueueSize); publisher.publish(m); @@ -774,23 +778,24 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { static ros::Publisher velocityPublisher = nh->advertise("fix_velocity", kROSQueueSize); - if (m.iTOW == last_nav_pos_.iTOW) + if (m.i_tow == last_nav_pos_.i_tow) { velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh - else + } else { velocity_.header.stamp = ros::Time::now(); // create a new timestamp + } velocity_.header.frame_id = frame_id; // convert to XYZ linear velocity - velocity_.twist.twist.linear.x = m.velE / 100.0; - velocity_.twist.twist.linear.y = m.velN / 100.0; - velocity_.twist.twist.linear.z = -m.velD / 100.0; + velocity_.twist.twist.linear.x = m.vel_e / 100.0; + velocity_.twist.twist.linear.y = m.vel_n / 100.0; + velocity_.twist.twist.linear.z = -m.vel_d / 100.0; - const double varSpeed = pow(m.sAcc / 100.0, 2); + const double var_speed = pow(m.s_acc / 100.0, 2); const int cols = 6; - velocity_.twist.covariance[cols * 0 + 0] = varSpeed; - velocity_.twist.covariance[cols * 1 + 1] = varSpeed; - velocity_.twist.covariance[cols * 2 + 2] = varSpeed; + velocity_.twist.covariance[cols * 0 + 0] = var_speed; + velocity_.twist.covariance[cols * 1 + 1] = var_speed; + velocity_.twist.covariance[cols * 2 + 2] = var_speed; velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported velocityPublisher.publish(velocity_); @@ -798,7 +803,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { } void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if(enabled["nav_sol"]) { + if (enabled["nav_sol"]) { static ros::Publisher publisher = nh->advertise("navsol", kROSQueueSize); publisher.publish(m); @@ -820,7 +825,7 @@ void UbloxFirmware7::getRosParams() { nh->param("gnss/glonass", enable_glonass_, false); nh->param("gnss/qzss", enable_qzss_, false); getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); if(enable_gps_ && !supportsGnss("GPS")) @@ -851,13 +856,13 @@ void UbloxFirmware7::getRosParams() { if (set_nmea_) { bool compat, consider; - if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) + if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) + if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) @@ -886,16 +891,16 @@ void UbloxFirmware7::getRosParams() { cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId); + getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); } } @@ -904,24 +909,24 @@ bool UbloxFirmware7::configureUblox() { ublox_msgs::CfgGNSS cfgGNSSRead; if (gps.poll(cfgGNSSRead)) { ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.numTrkChHw); - ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.numTrkChUse); + ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); + ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); } else { throw std::runtime_error("Failed to read the GNSS config."); } ublox_msgs::CfgGNSS cfgGNSSWrite; - cfgGNSSWrite.numConfigBlocks = 1; // do services one by one - cfgGNSSWrite.numTrkChHw = cfgGNSSRead.numTrkChHw; - cfgGNSSWrite.numTrkChUse = cfgGNSSRead.numTrkChUse; - cfgGNSSWrite.msgVer = 0; + cfgGNSSWrite.num_config_blocks = 1; // do services one by one + cfgGNSSWrite.num_trk_ch_hw = cfgGNSSRead.num_trk_ch_hw; + cfgGNSSWrite.num_trk_ch_use = cfgGNSSRead.num_trk_ch_use; + cfgGNSSWrite.msg_ver = 0; // configure GLONASS - if(supportsGnss("GLO")) { - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_GLONASS; - block.resTrkCh = block.RES_TRK_CH_GLONASS; - block.maxTrkCh = block.MAX_TRK_CH_GLONASS; + if (supportsGnss("GLO")) { + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_GLONASS; + block.res_trk_ch = block.RES_TRK_CH_GLONASS; + block.max_trk_ch = block.MAX_TRK_CH_GLONASS; block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; cfgGNSSWrite.blocks.push_back(block); if (!gps.configure(cfgGNSSWrite)) { @@ -931,12 +936,12 @@ bool UbloxFirmware7::configureUblox() { } } - if(supportsGnss("QZSS")) { + if (supportsGnss("QZSS")) { // configure QZSS - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_QZSS; - block.resTrkCh = block.RES_TRK_CH_QZSS; - block.maxTrkCh = block.MAX_TRK_CH_QZSS; + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_QZSS; + block.res_trk_ch = block.RES_TRK_CH_QZSS; + block.max_trk_ch = block.MAX_TRK_CH_QZSS; block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; cfgGNSSWrite.blocks[0] = block; if (!gps.configure(cfgGNSSWrite)) { @@ -948,10 +953,10 @@ bool UbloxFirmware7::configureUblox() { if(supportsGnss("SBAS")) { // configure SBAS - ublox_msgs::CfgGNSS_Block block; - block.gnssId = block.GNSS_ID_SBAS; - block.resTrkCh = block.RES_TRK_CH_SBAS; - block.maxTrkCh = block.MAX_TRK_CH_SBAS; + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_SBAS; + block.res_trk_ch = block.RES_TRK_CH_SBAS; + block.max_trk_ch = block.MAX_TRK_CH_SBAS; block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps.configure(cfgGNSSWrite)) { @@ -961,8 +966,9 @@ bool UbloxFirmware7::configureUblox() { } } - if(set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); + } return true; } @@ -1010,7 +1016,7 @@ void UbloxFirmware8::getRosParams() { nh->param("gnss/sbas", enable_sbas_, false); // QZSS Signal Configuration getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA); + ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !supportsGnss("GPS")) ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", @@ -1047,13 +1053,13 @@ void UbloxFirmware8::getRosParams() { cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set - if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion)) + if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV)) + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering)) + if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); if (!nh->getParam("nmea/compat", compat)) @@ -1086,32 +1092,33 @@ void UbloxFirmware8::getRosParams() { cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; nh->param("nmea/gnssToFilter/beidou", temp, false); - cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId); + getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); - std::vector bdsTalkerId; - getRosUint("nmea/bds_talker_id", bdsTalkerId); - cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0]; - cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1]; + std::vector bds_talker_id; + getRosUint("nmea/bds_talker_id", bds_talker_id); + cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; + cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; } } bool UbloxFirmware8::configureUblox() { - if(clear_bbr_) { + if (clear_bbr_) { // clear flash memory - if(!gps.clearBbr()) + if (!gps.clearBbr()) { ROS_ERROR("u-blox failed to clear flash memory"); + } } // // Configure the GNSS, only if the configuration is different @@ -1120,8 +1127,8 @@ bool UbloxFirmware8::configureUblox() { ublox_msgs::CfgGNSS cfg_gnss; if (gps.poll(cfg_gnss)) { ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.numTrkChHw); - ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.numTrkChUse); + ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); } else { throw std::runtime_error("Failed to read the GNSS config."); } @@ -1129,37 +1136,37 @@ bool UbloxFirmware8::configureUblox() { // Then, check the configuration for each GNSS. If it is different, change it. bool correct = true; for (int i = 0; i < cfg_gnss.blocks.size(); i++) { - ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i]; - if (block.gnssId == block.GNSS_ID_GPS + ublox_msgs::CfgGNSSBlock block = cfg_gnss.blocks[i]; + if (block.gnss_id == block.GNSS_ID_GPS && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_SBAS + } else if (block.gnss_id == block.GNSS_ID_SBAS && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_GALILEO + } else if (block.gnss_id == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_BEIDOU + } else if (block.gnss_id == block.GNSS_ID_BEIDOU && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnssId == block.GNSS_ID_IMES + } else if (block.gnss_id == block.GNSS_ID_IMES && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnssId == block.GNSS_ID_QZSS + } else if (block.gnss_id == block.GNSS_ID_QZSS && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) || (enable_qzss_ && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { @@ -1174,7 +1181,7 @@ bool UbloxFirmware8::configureUblox() { if (enable_qzss_) // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } else if (block.gnssId == block.GNSS_ID_GLONASS + } else if (block.gnss_id == block.GNSS_ID_GLONASS && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = @@ -1507,7 +1514,7 @@ bool HpgRefProduct::configureUblox() { throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY + if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { setTimeMode(); return true; @@ -1605,20 +1612,20 @@ void HpgRefProduct::tmode3Diagnostics( stat.message = "Survey-In active and valid"; } - stat.add("iTOW [ms]", last_nav_svin_.iTOW); + stat.add("iTOW [ms]", last_nav_svin_.i_tow); stat.add("Duration [s]", last_nav_svin_.dur); stat.add("# observations", last_nav_svin_.obs); - stat.add("Mean X [m]", last_nav_svin_.meanX * 1e-2); - stat.add("Mean Y [m]", last_nav_svin_.meanY * 1e-2); - stat.add("Mean Z [m]", last_nav_svin_.meanZ * 1e-2); - stat.add("Mean X HP [m]", last_nav_svin_.meanXHP * 1e-4); - stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4); - stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4); - stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4); - } else if(mode_ == FIXED) { + stat.add("Mean X [m]", last_nav_svin_.mean_x * 1e-2); + stat.add("Mean Y [m]", last_nav_svin_.mean_y * 1e-2); + stat.add("Mean Z [m]", last_nav_svin_.mean_z * 1e-2); + stat.add("Mean X HP [m]", last_nav_svin_.mean_xhp * 1e-4); + stat.add("Mean Y HP [m]", last_nav_svin_.mean_yhp * 1e-4); + stat.add("Mean Z HP [m]", last_nav_svin_.mean_zhp * 1e-4); + stat.add("Mean Accuracy [m]", last_nav_svin_.mean_acc * 1e-4); + } else if (mode_ == FIXED) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed Position"; - } else if(mode_ == TIME) { + } else if (mode_ == TIME) { stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Time"; } @@ -1635,8 +1642,9 @@ void HpgRovProduct::getRosParams() { bool HpgRovProduct::configureUblox() { // Configure the DGNSS - if(!gps.setDgnss(dgnss_mode_)) + if (!gps.setDgnss(dgnss_mode_)) { throw std::runtime_error(std::string("Failed to Configure DGNSS")); + } return true; } @@ -1660,7 +1668,7 @@ void HpgRovProduct::initializeRosDiagnostics() { void HpgRovProduct::carrierPhaseDiagnostics( diagnostic_updater::DiagnosticStatusWrapper& stat) { uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; - stat.add("iTow", last_rel_pos_.iTow); + stat.add("iTow", last_rel_pos_.i_tow); if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { @@ -1674,20 +1682,20 @@ void HpgRovProduct::carrierPhaseDiagnostics( stat.level = diagnostic_msgs::DiagnosticStatus::OK; stat.message = "Fixed"; } - stat.add("Ref Station ID", last_rel_pos_.refStationId); - - double rel_pos_n = (last_rel_pos_.relPosN - + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.relPosE - + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.relPosD - + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2; + stat.add("Ref Station ID", last_rel_pos_.ref_station_id); + + double rel_pos_n = (last_rel_pos_.rel_pos_n + + (last_rel_pos_.rel_pos_hpn * 1e-2)) * 1e-2; + double rel_pos_e = (last_rel_pos_.rel_pos_e + + (last_rel_pos_.rel_pos_hpe * 1e-2)) * 1e-2; + double rel_pos_d = (last_rel_pos_.rel_pos_d + + (last_rel_pos_.rel_pos_hpd * 1e-2)) * 1e-2; stat.add("Relative Position N [m]", rel_pos_n); - stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4); + stat.add("Relative Accuracy N [m]", last_rel_pos_.acc_n * 1e-4); stat.add("Relative Position E [m]", rel_pos_e); - stat.add("Relative Accuracy E [m]", last_rel_pos_.accE * 1e-4); + stat.add("Relative Accuracy E [m]", last_rel_pos_.acc_e * 1e-4); stat.add("Relative Position D [m]", rel_pos_d); - stat.add("Relative Accuracy D [m]", last_rel_pos_.accD * 1e-4); + stat.add("Relative Accuracy D [m]", last_rel_pos_.acc_d * 1e-4); } } @@ -1734,7 +1742,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; - double heading = static_cast(m.relPosHeading) * 1e-5 / 180.0 * M_PI; + double heading = static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI; tf::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; @@ -1744,7 +1752,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // Only heading is reported with an accuracy in 0.1mm units imu_.orientation_covariance[0] = 1000.0; imu_.orientation_covariance[4] = 1000.0; - imu_.orientation_covariance[8] = pow(m.accHeading / 10000.0, 2); + imu_.orientation_covariance[8] = pow(m.acc_heading / 10000.0, 2); imu_pub.publish(imu_); } @@ -1804,11 +1812,11 @@ void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { nh->advertise("interrupt_time", kROSQueueSize); // create time ref message and put in the data - t_ref_.header.seq = m.risingEdgeCount; + t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); t_ref_.header.frame_id = frame_id; - t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + t_ref_.time_ref = ros::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); std::ostringstream src; src << "TIM" << int(m.ch); diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp index d17b8c94..7c4eeb2d 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp @@ -51,16 +51,16 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::CfgDAT_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.datumNum); - stream.next(m.datumName); - stream.next(m.majA); + stream.next(m.datum_num); + stream.next(m.datum_name); + stream.next(m.maj_a); stream.next(m.flat); - stream.next(m.dX); - stream.next(m.dY); - stream.next(m.dZ); - stream.next(m.rotX); - stream.next(m.rotY); - stream.next(m.rotZ); + stream.next(m.d_x); + stream.next(m.d_y); + stream.next(m.d_z); + stream.next(m.rot_x); + stream.next(m.rot_y); + stream.next(m.rot_z); stream.next(m.scale); } @@ -74,14 +74,14 @@ struct Serializer > { const ublox_msgs::CfgDAT_ &m) { ros::serialization::OStream stream(data, size); // ignores datumNum & datumName - stream.next(m.majA); + stream.next(m.maj_a); stream.next(m.flat); - stream.next(m.dX); - stream.next(m.dY); - stream.next(m.dZ); - stream.next(m.rotX); - stream.next(m.rotY); - stream.next(m.rotZ); + stream.next(m.d_x); + stream.next(m.d_y); + stream.next(m.d_z); + stream.next(m.rot_x); + stream.next(m.rot_y); + stream.next(m.rot_z); stream.next(m.scale); } }; @@ -94,31 +94,31 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::CfgGNSS_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.msgVer); - stream.next(m.numTrkChHw); - stream.next(m.numTrkChUse); - stream.next(m.numConfigBlocks); - m.blocks.resize(m.numConfigBlocks); + stream.next(m.msg_ver); + stream.next(m.num_trk_ch_hw); + stream.next(m.num_trk_ch_use); + stream.next(m.num_config_blocks); + m.blocks.resize(m.num_config_blocks); for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::deserialize(stream, m.blocks[i]); } } static uint32_t serializedLength(const ublox_msgs::CfgGNSS_ &m) { - return 4 + 8 * m.numConfigBlocks; + return 4 + 8 * m.num_config_blocks; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::CfgGNSS_ &m) { - if (m.blocks.size() != m.numConfigBlocks) { - ROS_ERROR("CfgGNSS numConfigBlocks must equal blocks size"); + if (m.blocks.size() != m.num_config_blocks) { + ROS_ERROR("CfgGNSS num_config_blocks must equal blocks size"); } ros::serialization::OStream stream(data, size); - stream.next(m.msgVer); - stream.next(m.numTrkChHw); - stream.next(m.numTrkChUse); + stream.next(m.msg_ver); + stream.next(m.num_trk_ch_hw); + stream.next(m.num_trk_ch_use); stream.next( - static_cast::_numConfigBlocks_type>(m.blocks.size())); + static_cast::_num_config_blocks_type>(m.blocks.size())); for (std::size_t i = 0; i < m.blocks.size(); ++i) { ros::serialization::serialize(stream, m.blocks[i]); } @@ -188,8 +188,8 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::MonVER_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.swVersion); - stream.next(m.hwVersion); + stream.next(m.sw_version); + stream.next(m.hw_version); m.extension.clear(); int N = (count - 40) / 30; @@ -209,8 +209,8 @@ struct Serializer > { static void write(uint8_t *data, uint32_t size, const ublox_msgs::MonVER_ &m) { ros::serialization::OStream stream(data, size); - stream.next(m.swVersion); - stream.next(m.hwVersion); + stream.next(m.sw_version); + stream.next(m.hw_version); for (std::size_t i = 0; i < m.extension.size(); ++i) { ros::serialization::serialize(stream, m.extension[i]); } @@ -225,34 +225,34 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::NavDGPS_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.age); - stream.next(m.baseId); - stream.next(m.baseHealth); - stream.next(m.numCh); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(m.num_ch); stream.next(m.status); stream.next(m.reserved1); - m.sv.resize(m.numCh); + m.sv.resize(m.num_ch); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); } } static uint32_t serializedLength(const ublox_msgs::NavDGPS_ &m) { - return 16 + 12 * m.numCh; + return 16 + 12 * m.num_ch; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::NavDGPS_ &m) { - if (m.sv.size() != m.numCh) { + if (m.sv.size() != m.num_ch) { ROS_ERROR("NavDGPS numCh must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.age); - stream.next(m.baseId); - stream.next(m.baseHealth); - stream.next(static_cast::_numCh_type>(m.sv.size())); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(static_cast::_num_ch_type>(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); for (std::size_t i = 0; i < m.sv.size(); ++i) { @@ -270,7 +270,7 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::NavSBAS_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.geo); stream.next(m.mode); stream.next(m.sys); @@ -293,7 +293,7 @@ struct Serializer > { ROS_ERROR("NavSBAS cnt must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.geo); stream.next(m.mode); stream.next(m.sys); @@ -314,29 +314,29 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::NavSAT_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.version); - stream.next(m.numSvs); + stream.next(m.num_svs); stream.next(m.reserved0); - m.sv.resize(m.numSvs); + m.sv.resize(m.num_svs); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); } } static uint32_t serializedLength(const ublox_msgs::NavSAT_ &m) { - return 8 + 12 * m.numSvs; + return 8 + 12 * m.num_svs; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::NavSAT_ &m) { - if (m.sv.size() != m.numSvs) { - ROS_ERROR("NavSAT numSvs must equal sv size"); + if (m.sv.size() != m.num_svs) { + ROS_ERROR("NavSAT num_svs must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.version); - stream.next(static_cast::_numSvs_type>(m.sv.size())); + stream.next(static_cast::_num_svs_type>(m.sv.size())); stream.next(m.reserved0); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); @@ -352,29 +352,29 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::NavSVINFO_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); - stream.next(m.numCh); - stream.next(m.globalFlags); + stream.next(m.i_tow); + stream.next(m.num_ch); + stream.next(m.global_flags); stream.next(m.reserved2); - m.sv.resize(m.numCh); + m.sv.resize(m.num_ch); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); } } static uint32_t serializedLength(const ublox_msgs::NavSVINFO_ &m) { - return 8 + 12 * m.numCh; + return 8 + 12 * m.num_ch; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::NavSVINFO_ &m) { - if (m.sv.size() != m.numCh) { - ROS_ERROR("NavSVINFO numCh must equal sv size"); + if (m.sv.size() != m.num_ch) { + ROS_ERROR("NavSVINFO num_ch must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); - stream.next(static_cast::_numCh_type>(m.sv.size())); - stream.next(m.globalFlags); + stream.next(m.i_tow); + stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(m.global_flags); stream.next(m.reserved2); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); @@ -390,29 +390,29 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::RxmRAW_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcvTOW); + stream.next(m.rcv_tow); stream.next(m.week); - stream.next(m.numSV); + stream.next(m.num_sv); stream.next(m.reserved1); - m.sv.resize(m.numSV); + m.sv.resize(m.num_sv); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); } } static uint32_t serializedLength(const ublox_msgs::RxmRAW_ &m) { - return 8 + 24 * m.numSV; + return 8 + 24 * m.num_sv; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::RxmRAW_ &m) { - if (m.sv.size() != m.numSV) { - ROS_ERROR("RxmRAW numSV must equal sv size"); + if (m.sv.size() != m.num_sv) { + ROS_ERROR("RxmRAW num_sv must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.rcvTOW); + stream.next(m.rcv_tow); stream.next(m.week); - stream.next(static_cast::_numSV_type>(m.sv.size())); + stream.next(static_cast::_num_sv_type>(m.sv.size())); stream.next(m.reserved1); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); @@ -428,34 +428,34 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::RxmRAWX_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcvTOW); + stream.next(m.rcv_tow); stream.next(m.week); - stream.next(m.leapS); - stream.next(m.numMeas); - stream.next(m.recStat); + stream.next(m.leap_s); + stream.next(m.num_meas); + stream.next(m.rec_stat); stream.next(m.version); stream.next(m.reserved1); - m.meas.resize(m.numMeas); + m.meas.resize(m.num_meas); for (std::size_t i = 0; i < m.meas.size(); ++i) { ros::serialization::deserialize(stream, m.meas[i]); } } static uint32_t serializedLength(const ublox_msgs::RxmRAWX_ &m) { - return 16 + 32 * m.numMeas; + return 16 + 32 * m.num_meas; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::RxmRAWX_ &m) { - if (m.meas.size() != m.numMeas) { - ROS_ERROR("RxmRAWX numMeas must equal meas size"); + if (m.meas.size() != m.num_meas) { + ROS_ERROR("RxmRAWX num_meas must equal meas size"); } ros::serialization::OStream stream(data, size); - stream.next(m.rcvTOW); + stream.next(m.rcv_tow); stream.next(m.week); - stream.next(m.leapS); - stream.next(static_cast::_numMeas_type>(m.meas.size())); - stream.next(m.recStat); + stream.next(m.leap_s); + stream.next(static_cast::_num_meas_type>(m.meas.size())); + stream.next(m.rec_stat); stream.next(m.version); stream.next(m.reserved1); for (std::size_t i = 0; i < m.meas.size(); ++i) { @@ -472,35 +472,35 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::RxmSFRBX_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.gnssId); - stream.next(m.svId); + stream.next(m.gnss_id); + stream.next(m.sv_id); stream.next(m.reserved0); - stream.next(m.freqId); - stream.next(m.numWords); + stream.next(m.freq_id); + stream.next(m.num_words); stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); - m.dwrd.resize(m.numWords); + m.dwrd.resize(m.num_words); for (std::size_t i = 0; i < m.dwrd.size(); ++i) { ros::serialization::deserialize(stream, m.dwrd[i]); } } static uint32_t serializedLength(const ublox_msgs::RxmSFRBX_ &m) { - return 8 + 4 * m.numWords; + return 8 + 4 * m.num_words; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::RxmSFRBX_ &m) { - if (m.dwrd.size() != m.numWords) { - ROS_ERROR("RxmSFRBX numWords must equal dwrd size"); + if (m.dwrd.size() != m.num_words) { + ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); } ros::serialization::OStream stream(data, size); - stream.next(m.gnssId); - stream.next(m.svId); + stream.next(m.gnss_id); + stream.next(m.sv_id); stream.next(m.reserved0); - stream.next(m.freqId); - stream.next(static_cast::_numWords_type>(m.dwrd.size())); + stream.next(m.freq_id); + stream.next(static_cast::_num_words_type>(m.dwrd.size())); stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); @@ -518,30 +518,30 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::RxmSVSI_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.week); - stream.next(m.numVis); - stream.next(m.numSV); - m.sv.resize(m.numSV); + stream.next(m.num_vis); + stream.next(m.num_sv); + m.sv.resize(m.num_sv); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::deserialize(stream, m.sv[i]); } } static uint32_t serializedLength(const ublox_msgs::RxmSVSI_ &m) { - return 8 + 6 * m.numSV; + return 8 + 6 * m.num_sv; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::RxmSVSI_ &m) { - if (m.sv.size() != m.numSV) { - ROS_ERROR("RxmSVSI numSV must equal sv size"); + if (m.sv.size() != m.num_sv) { + ROS_ERROR("RxmSVSI num_sv must equal sv size"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.week); - stream.next(m.numVis); - stream.next(static_cast::_numSV_type>(m.sv.size())); + stream.next(m.num_vis); + stream.next(static_cast::_num_sv_type>(m.sv.size())); for (std::size_t i = 0; i < m.sv.size(); ++i) { ros::serialization::serialize(stream, m.sv[i]); } @@ -748,7 +748,7 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::EsfMEAS_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.timeTag); + stream.next(m.time_tag); stream.next(m.flags); stream.next(m.id); @@ -761,26 +761,26 @@ struct Serializer > { } // Optional block if (calib_valid) { - m.calibTtag.resize(1); - ros::serialization::deserialize(stream, m.calibTtag[0]); + m.calib_t_tag.resize(1); + ros::serialization::deserialize(stream, m.calib_t_tag[0]); } } static uint32_t serializedLength(const ublox_msgs::EsfMEAS_ &m) { - return 4 + 8 * m.data.size() + 4 * m.calibTtag.size(); + return 4 + 8 * m.data.size() + 4 * m.calib_t_tag.size(); } static void write(uint8_t *data, uint32_t size, const ublox_msgs::EsfMEAS_ &m) { ros::serialization::OStream stream(data, size); - stream.next(m.timeTag); + stream.next(m.time_tag); stream.next(m.flags); stream.next(m.id); for (std::size_t i = 0; i < m.data.size(); ++i) { ros::serialization::serialize(stream, m.data[i]); } - for (std::size_t i = 0; i < m.calibTtag.size(); ++i) { - ros::serialization::serialize(stream, m.calibTtag[i]); + for (std::size_t i = 0; i < m.calib_t_tag.size(); ++i) { + ros::serialization::serialize(stream, m.calib_t_tag[i]); } } }; @@ -824,32 +824,32 @@ struct Serializer > { static void read(const uint8_t *data, uint32_t count, ublox_msgs::EsfSTATUS_ &m) { ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.version); - stream.next(m.fusionMode); + stream.next(m.fusion_mode); stream.next(m.reserved2); - stream.next(m.numSens); - m.sens.resize(m.numSens); + stream.next(m.num_sens); + m.sens.resize(m.num_sens); for (std::size_t i = 0; i < m.sens.size(); ++i) { ros::serialization::deserialize(stream, m.sens[i]); } } static uint32_t serializedLength(const ublox_msgs::EsfSTATUS_ &m) { - return 16 + 4 * m.numSens; + return 16 + 4 * m.num_sens; } static void write(uint8_t *data, uint32_t size, const ublox_msgs::EsfSTATUS_ &m) { - if (m.sens.size() != m.numSens) { - ROS_ERROR("Writing EsfSTATUS message: numSens must equal size of sens"); + if (m.sens.size() != m.num_sens) { + ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); } ros::serialization::OStream stream(data, size); - stream.next(m.iTOW); + stream.next(m.i_tow); stream.next(m.version); - stream.next(m.fusionMode); + stream.next(m.fusion_mode); stream.next(m.reserved2); - stream.next(static_cast::_numSens_type>(m.sens.size())); + stream.next(static_cast::_num_sens_type>(m.sens.size())); for (std::size_t i = 0; i < m.sens.size(); ++i) { ros::serialization::serialize(stream, m.sens[i]); } diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index a710a374..8602e222 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -53,9 +53,9 @@ #include #include #include -#include +#include #include -#include +#include #include #include #include @@ -71,7 +71,7 @@ #include #include #include -#include +#include #include #include #include @@ -86,7 +86,7 @@ #include #include -#include +#include #include #include @@ -101,7 +101,7 @@ #include #include #include -#include +#include #include diff --git a/ublox_msgs/msg/Ack.msg b/ublox_msgs/msg/Ack.msg index fb7aba6b..85b24831 100644 --- a/ublox_msgs/msg/Ack.msg +++ b/ublox_msgs/msg/Ack.msg @@ -8,5 +8,5 @@ uint8 CLASS_ID = 5 uint8 NACK_MESSAGE_ID = 0 uint8 ACK_MESSAGE_ID = 1 -uint8 clsID # Class ID of the (Not-)Acknowledged Message -uint8 msgID # Message ID of the (Not-)Acknowledged Message \ No newline at end of file +uint8 cls_id # Class ID of the (Not-)Acknowledged Message +uint8 msg_id # Message ID of the (Not-)Acknowledged Message diff --git a/ublox_msgs/msg/AidALM.msg b/ublox_msgs/msg/AidALM.msg index 1997ee54..b770f8f0 100644 --- a/ublox_msgs/msg/AidALM.msg +++ b/ublox_msgs/msg/AidALM.msg @@ -2,27 +2,27 @@ # GPS Aiding Almanach Input/Output Message # # All UBX-AID messages are deprecated; use UBX-MGA messages instead -# - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not -# available for the given SV. This may happen even if NAV-SVINFO and RXM-SVSI -# are indicating almanac availability as the internal data may not represent +# - If the WEEK Value is 0, DWRD0 to DWRD7 are not sent as the almanach is not +# available for the given SV. This may happen even if NAV-SVINFO and RXM-SVSI +# are indicating almanac availability as the internal data may not represent # the content of an original broadcast almanac (or only parts thereof). # - DWORD0 to DWORD7 contain the 8 words following the Hand-Over Word ( HOW ) -# from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or -# pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the +# from the GPS navigation message, either pages 1 to 24 of sub-frame 5 or +# pages 2 to 10 of subframe 4. See IS-GPS-200 for a full description of the # contents of the Almanac pages. -# - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of +# - In DWORD0 to DWORD7, the parity bits have been removed, and the 24 bits of # data are located in Bits 0 to 23. Bits 24 to 31 shall be ignored. -# - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, -# Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas +# - Example: Parameter e (Eccentricity) from Almanach Subframe 4/5, Word 3, +# Bits 69-84 within the subframe can be found in DWRD0, Bits 15-0 whereas # Bit 0 is the LSB. uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 48 -uint32 svid # SV ID for which the receiver shall return its +uint32 svid # SV ID for which the receiver shall return its # Almanac Data (Valid Range: 1 .. 32 or 51, 56, 63). uint32 week # Issue Date of Almanach (GPS week number) # Start of optional block -uint32[] dwrd # Almanach Words +uint32[] dwrd # Almanac Words # End of optional block diff --git a/ublox_msgs/msg/AidEPH.msg b/ublox_msgs/msg/AidEPH.msg index 81ffd014..45f3a8f2 100644 --- a/ublox_msgs/msg/AidEPH.msg +++ b/ublox_msgs/msg/AidEPH.msg @@ -20,14 +20,14 @@ uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 49 -uint32 svid # SV ID for which this ephemeris data is +uint32 svid # SV ID for which this ephemeris data is # (Valid Range: 1 .. 32). uint32 how # Hand-Over Word of first Subframe. This is # required if data is sent to the receiver. # 0 indicates that no Ephemeris Data is following. # Start of optional block -uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) -uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) -uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) +uint32[] sf1d # Subframe 1 Words 3..10 (SF1D0..SF1D7) +uint32[] sf2d # Subframe 2 Words 3..10 (SF2D0..SF2D7) +uint32[] sf3d # Subframe 3 Words 3..10 (SF3D0..SF3D7) # End of optional block diff --git a/ublox_msgs/msg/AidHUI.msg b/ublox_msgs/msg/AidHUI.msg index ee9c6aba..a76d945f 100644 --- a/ublox_msgs/msg/AidHUI.msg +++ b/ublox_msgs/msg/AidHUI.msg @@ -8,28 +8,28 @@ uint8 CLASS_ID = 11 uint8 MESSAGE_ID = 2 -uint32 health # Bitmask, every bit represents a GPS SV (1-32). - # If the bit is set the SV is healthy. -float64 utcA0 # UTC - parameter A0 -float64 utcA1 # UTC - parameter A1 -int32 utcTOW # UTC - reference time of week -int16 utcWNT # UTC - reference week number -int16 utcLS # UTC - time difference due to leap seconds before event -int16 utcWNF # UTC - week number when next leap second event occurs -int16 utcDN # UTC - day of week when next leap second event occurs -int16 utcLSF # UTC - time difference due to leap seconds after event -int16 utcSpare # UTC - Spare to ensure structure is a multiple of 4 - # bytes -float32 klobA0 # Klobuchar - alpha 0 [s] -float32 klobA1 # Klobuchar - alpha 1 [s/semicircle] -float32 klobA2 # Klobuchar - alpha 2 [s/semicircle^2] -float32 klobA3 # Klobuchar - alpha 3 [s/semicircle^3] -float32 klobB0 # Klobuchar - beta 0 [s] -float32 klobB1 # Klobuchar - beta 1 [s/semicircle] -float32 klobB2 # Klobuchar - beta 2 [s/semicircle^2] -float32 klobB3 # Klobuchar - beta 3 [s/semicircle^3] -uint32 flags # flags -uint32 FLAGS_HEALTH = 1 # Healthmask field in this message is valid -uint32 FLAGS_UTC = 2 # UTC parameter fields in this message are valid -uint32 FLAGS_KLOB = 4 # Klobuchar parameter fields in this message are - # valid +uint32 health # Bitmask, every bit represents a GPS SV (1-32). + # If the bit is set the SV is healthy. +float64 utc_a0 # UTC - parameter A0 +float64 utc_a1 # UTC - parameter A1 +int32 utc_tow # UTC - reference time of week +int16 utc_wnt # UTC - reference week number +int16 utc_ls # UTC - time difference due to leap seconds before event +int16 utc_wnf # UTC - week number when next leap second event occurs +int16 utc_dn # UTC - day of week when next leap second event occurs +int16 utc_lsf # UTC - time difference due to leap seconds after event +int16 utc_spare # UTC - Spare to ensure structure is a multiple of 4 + # bytes +float32 klob_a0 # Klobuchar - alpha 0 [s] +float32 klob_a1 # Klobuchar - alpha 1 [s/semicircle] +float32 klob_a2 # Klobuchar - alpha 2 [s/semicircle^2] +float32 klob_a3 # Klobuchar - alpha 3 [s/semicircle^3] +float32 klob_b0 # Klobuchar - beta 0 [s] +float32 klob_b1 # Klobuchar - beta 1 [s/semicircle] +float32 klob_b2 # Klobuchar - beta 2 [s/semicircle^2] +float32 klob_b3 # Klobuchar - beta 3 [s/semicircle^3] +uint32 flags # flags +uint32 FLAGS_HEALTH = 1 # Healthmask field in this message is valid +uint32 FLAGS_UTC = 2 # UTC parameter fields in this message are valid +uint32 FLAGS_KLOB = 4 # Klobuchar parameter fields in this message are + # valid diff --git a/ublox_msgs/msg/CfgANT.msg b/ublox_msgs/msg/CfgANT.msg index c26b1252..c7e24320 100644 --- a/ublox_msgs/msg/CfgANT.msg +++ b/ublox_msgs/msg/CfgANT.msg @@ -5,21 +5,21 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 19 -uint16 flags # Antenna Flag Mask +uint16 flags # Antenna Flag Mask uint16 FLAGS_SVCS = 1 # Enable Antenna Supply Voltage Control Signal uint16 FLAGS_SCD = 2 # Enable Short Circuit Detection uint16 FLAGS_OCD = 4 # Enable Open Circuit Detection -uint16 FLAGS_PDWN_ON_SCD = 8 # Power Down Antenna supply if Short Circuit is +uint16 FLAGS_PDWN_ON_SCD = 8 # Power Down Antenna supply if Short Circuit is # detected. (only in combination with Bit 1) uint16 FLAGS_RECOVERY = 16 # Enable automatic recovery from short state -uint16 pins # Antenna Pin Configuration -uint16 PIN_SWITCH_MASK = 31 # PIO-Pin used for switching antenna supply - # (internal to TIM-LP/TIM-LF) -uint16 PIN_SCD_MASK = 992 # PIO-Pin used for detecting a short in the - # antenna supply -uint16 PIN_OCD_MASK = 31744 # PIO-Pin used for detecting open/not connected - # antenna -uint16 PIN_RECONFIG = 32678 # if set to one, and this command is sent to the - # receiver, the receiver will reconfigure the - # pins as specified. +uint16 pins # Antenna Pin Configuration +uint16 PIN_SWITCH_MASK = 31 # PIO-Pin used for switching antenna supply + # (internal to TIM-LP/TIM-LF) +uint16 PIN_SCD_MASK = 992 # PIO-Pin used for detecting a short in the + # antenna supply +uint16 PIN_OCD_MASK = 31744 # PIO-Pin used for detecting open/not connected + # antenna +uint16 PIN_RECONFIG = 32678 # if set to one, and this command is sent to the + # receiver, the receiver will reconfigure the + # pins as specified. diff --git a/ublox_msgs/msg/CfgCFG.msg b/ublox_msgs/msg/CfgCFG.msg index 6f9cdf61..a83b7359 100644 --- a/ublox_msgs/msg/CfgCFG.msg +++ b/ublox_msgs/msg/CfgCFG.msg @@ -5,19 +5,19 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 9 -uint32 clearMask # Mask with configuration sub-sections to Clear - # (=Load Default Configurations to Permanent - # Configurations in non-volatile memory) -uint32 saveMask # Mask with configuration sub-section to Save - # (=Save Current Configuration to Non-volatile - # Memory) -uint32 loadMask # Mask with configuration sub-sections to Load - # (=Load Permanent Configurations from - # Non-volatile Memory to Current Configurations) +uint32 clear_mask # Mask with configuration sub-sections to Clear + # (=Load Default Configurations to Permanent + # Configurations in non-volatile memory) +uint32 save_mask # Mask with configuration sub-section to Save + # (=Save Current Configuration to Non-volatile + # Memory) +uint32 load_mask # Mask with configuration sub-sections to Load + # (=Load Permanent Configurations from + # Non-volatile Memory to Current Configurations) -uint32 MASK_IO_PORT = 1 # Communications port settings. Modifying this - # sub-section results in an IO system reset. - # Because of this undefined data may be output +uint32 MASK_IO_PORT = 1 # Communications port settings. Modifying this + # sub-section results in an IO system reset. + # Because of this undefined data may be output # for a short period of time after receiving the # message. uint32 MASK_MSG_CONF = 2 # Message Configuration @@ -28,10 +28,10 @@ uint32 MASK_SEN_CONF = 256 # Sensor Interface Configuration, protocol >= 19 uint32 MASK_RINV_CONF = 512 # Remote Inventory Configuration uint32 MASK_ANT_CONF = 1024 # Antenna Configuration uint32 MASK_LOG_CONF = 2048 # Logging Configuration -uint32 MASK_FTS_CONF = 4096 # FTS Configuration. Only applicable to the +uint32 MASK_FTS_CONF = 4096 # FTS Configuration. Only applicable to the # FTS product variant. -uint8 deviceMask # Mask which selects the devices for this command +uint8 device_mask # Mask which selects the devices for this command uint8 DEV_BBR = 1 # device battery backed RAM uint8 DEV_FLASH = 2 # device Flash uint8 DEV_EEPROM = 4 # device EEPROM diff --git a/ublox_msgs/msg/CfgDAT.msg b/ublox_msgs/msg/CfgDAT.msg index a659d7bb..cd8d57fa 100644 --- a/ublox_msgs/msg/CfgDAT.msg +++ b/ublox_msgs/msg/CfgDAT.msg @@ -1,37 +1,37 @@ # CFG-DAT (0x06 0x06) # Set User-defined Datum -# +# # For more information see the description of Geodetic Systems and Frames. -# +# uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 6 # Only for GET, these values are not used for write -uint16 datumNum # Datum Number: 0 = WGS84, 0xFFFF = user-defined +uint16 datum_num # Datum Number: 0 = WGS84, 0xFFFF = user-defined uint16 DATUM_NUM_WGS84 = 0 uint16 DATUM_NUM_USER = 65535 -uint8[6] datumName # ASCII String: WGS84 or USER +uint8[6] datum_name # ASCII String: WGS84 or USER -float64 majA # Semi-major Axis [m] - # accepted range = 6,300,000.0 to 6,500,000.0 meters -float64 flat # 1.0 / Flattening - # accepted range is 0.0 to 500.0 +float64 maj_a # Semi-major Axis [m] + # accepted range = 6,300,000.0 to 6,500,000.0 meters +float64 flat # 1.0 / Flattening + # accepted range is 0.0 to 500.0 -float32 dX # X Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters -float32 dY # Y Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters -float32 dZ # Z Axis shift at the origin [m] - # accepted range is +/- 5000.0 meters +float32 d_x # X Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters +float32 d_y # Y Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters +float32 d_z # Z Axis shift at the origin [m] + # accepted range is +/- 5000.0 meters -float32 rotX # Rotation about the X Axis [s] - # accepted range is +/- 20.0 milli-arc seconds -float32 rotY # Rotation about the Y Axis [s] - # accepted range is +/- 20.0 milli-arc seconds -float32 rotZ # Rotation about the Z Axis [s] - # accepted range is +/- 20.0 milli-arc seconds +float32 rot_x # Rotation about the X Axis [s] + # accepted range is +/- 20.0 milli-arc seconds +float32 rot_y # Rotation about the Y Axis [s] + # accepted range is +/- 20.0 milli-arc seconds +float32 rot_z # Rotation about the Z Axis [s] + # accepted range is +/- 20.0 milli-arc seconds -float32 scale # Scale change [ppm] - # accepted range is 0.0 to 50.0 parts per million \ No newline at end of file +float32 scale # Scale change [ppm] + # accepted range is 0.0 to 50.0 parts per million diff --git a/ublox_msgs/msg/CfgDGNSS.msg b/ublox_msgs/msg/CfgDGNSS.msg index 6baa38e8..7adbce74 100644 --- a/ublox_msgs/msg/CfgDGNSS.msg +++ b/ublox_msgs/msg/CfgDGNSS.msg @@ -1,7 +1,7 @@ # CFG-DGNSS (0x06 0x70) # DGNSS configuration # -# This message allows the user to configure the DGNSS configuration of the +# This message allows the user to configure the DGNSS configuration of the # receiver. # Supported on: # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 (only @@ -10,9 +10,9 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 112 -uint8 dgnssMode # Specifies differential mode: +uint8 dgnss_mode # Specifies differential mode: uint8 DGNSS_MODE_RTK_FLOAT = 2 # RTK float: No attempts are made to fix # ambiguities. uint8 DGNSS_MODE_RTK_FIXED = 3 # RTK fixed: Ambiguities are fixed whenever # possible. -uint8[3] reserved0 # Reserved \ No newline at end of file +uint8[3] reserved0 # Reserved diff --git a/ublox_msgs/msg/CfgGNSS.msg b/ublox_msgs/msg/CfgGNSS.msg index f7a8c4aa..50a20989 100644 --- a/ublox_msgs/msg/CfgGNSS.msg +++ b/ublox_msgs/msg/CfgGNSS.msg @@ -2,43 +2,43 @@ # GNSS Configuration # # Gets or sets the GNSS system channel sharing configuration. -# If the receiver is sent a valid new configuration, it will respond with a +# If the receiver is sent a valid new configuration, it will respond with a # UBX-ACK-ACK message and immediately change to the new configuration. Otherwise -# the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing +# the receiver will reject the request, by issuing a UBX-ACK-NAK and continuing # operation with the previous configuration. # Configuration requirements: -# - It is necessary for at least one major GNSS to be enabled, after applying +# - It is necessary for at least one major GNSS to be enabled, after applying # the new configuration to the current one. -# - It is also required that at least 4 tracking channels are available to each +# - It is also required that at least 4 tracking channels are available to each # enabled major GNSS, i.e. maxTrkCh must have a minimum value of 4 for each # enabled major GNSS. -# - The number of tracking channels in use must not exceed the number of -# tracking channels available in hardware, and the sum of all reserved +# - The number of tracking channels in use must not exceed the number of +# tracking channels available in hardware, and the sum of all reserved # tracking channels needs to be less than or equal to the number of tracking # channels in use. # Notes: # - To avoid cross-correlation issues, it is recommended that GPS and QZSS are # always both enabled or both disabled. -# - Polling this message returns the configuration of all supported GNSS, -# whether enabled or not; it may also include GNSS unsupported by the +# - Polling this message returns the configuration of all supported GNSS, +# whether enabled or not; it may also include GNSS unsupported by the # particular product, but in such cases the enable flag will always be unset. -# - See section GNSS Configuration for a discussion of the use of this message +# - See section GNSS Configuration for a discussion of the use of this message # and section Satellite Numbering for a description of the GNSS IDs available -# - Configuration specific to the GNSS system can be done via other messages +# - Configuration specific to the GNSS system can be done via other messages # (e.g. UBX-CFG-SBAS). # uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 62 -uint8 msgVer # Message version (= 0 for this version) -uint8 numTrkChHw # Number of tracking channels in hardware (read only) -uint8 numTrkChUse # (Read only in protocol versions greater than 23) - # Number of tracking channels to use (<= numTrkChHw) - # If 0xFF, then number of tracking channels to use will - # be set to numTrkChHw -uint8 numConfigBlocks # Number of configuration blocks following +uint8 msg_ver # Message version (= 0 for this version) +uint8 num_trk_ch_hw # Number of tracking channels in hardware (read only) +uint8 num_trk_ch_use # (Read only in protocol versions greater than 23) + # Number of tracking channels to use (<= numTrkChHw) + # If 0xFF, then number of tracking channels to use will + # be set to numTrkChHw +uint8 num_config_blocks # Number of configuration blocks following # Start of repeated block (numConfigBlocks times) -CfgGNSS_Block[] blocks -# End of repeated block \ No newline at end of file +CfgGNSSBlock[] blocks +# End of repeated block diff --git a/ublox_msgs/msg/CfgGNSS_Block.msg b/ublox_msgs/msg/CfgGNSSBlock.msg similarity index 56% rename from ublox_msgs/msg/CfgGNSS_Block.msg rename to ublox_msgs/msg/CfgGNSSBlock.msg index fcc36c66..4e4264fc 100644 --- a/ublox_msgs/msg/CfgGNSS_Block.msg +++ b/ublox_msgs/msg/CfgGNSSBlock.msg @@ -1,7 +1,7 @@ # see Cfg-GNSS message # -uint8 gnssId # System identifier (see Satellite Numbering) +uint8 gnss_id # System identifier (see Satellite Numbering) uint8 GNSS_ID_GPS = 0 uint8 GNSS_ID_SBAS = 1 @@ -11,29 +11,29 @@ uint8 GNSS_ID_IMES = 4 uint8 GNSS_ID_QZSS = 5 uint8 GNSS_ID_GLONASS = 6 -uint8 resTrkCh # (Read only in protocol versions greater than 23) - # Number of reserved (minimum) tracking channels - # for this GNSS system +uint8 res_trk_ch # (Read only in protocol versions greater than 23) + # Number of reserved (minimum) tracking channels + # for this GNSS system uint8 RES_TRK_CH_GPS = 8 uint8 RES_TRK_CH_QZSS = 0 uint8 RES_TRK_CH_SBAS = 0 uint8 RES_TRK_CH_GLONASS = 8 -uint8 maxTrkCh # (Read only in protocol versions greater than 23) - # Maximum number of tracking channels used for this - # system. Must be > 0, >= resTrkChn, <= numTrkChUse and - # <= maximum number of tracking channels supported for - # this system -uint8 MAX_TRK_CH_MAJOR_MIN = 4 # maxTrkCh must have this minimum value +uint8 max_trk_ch # (Read only in protocol versions greater than 23) + # Maximum number of tracking channels used for this + # system. Must be > 0, >= resTrkChn, <= numTrkChUse and + # <= maximum number of tracking channels supported for + # this system +uint8 MAX_TRK_CH_MAJOR_MIN = 4 # max_trk_ch must have this minimum value # for each enabled major GNSS uint8 MAX_TRK_CH_GPS = 16 uint8 MAX_TRK_CH_GLONASS = 14 uint8 MAX_TRK_CH_QZSS = 3 uint8 MAX_TRK_CH_SBAS = 3 -uint8 reserved1 # Reserved +uint8 reserved1 # Reserved -uint32 flags # Bitfield of flags. At least one signal must be - # configured in every enabled system. +uint32 flags # Bitfield of flags. At least one signal must be + # configured in every enabled system. uint32 FLAGS_ENABLE = 1 # Enable this system uint32 FLAGS_SIG_CFG_MASK = 16711680 # Signal configuration mask uint32 SIG_CFG_GPS_L1CA = 65536 # When gnssId is 0 (GPS) @@ -41,14 +41,14 @@ uint32 SIG_CFG_GPS_L1CA = 65536 # When gnssId is 0 (GPS) uint32 SIG_CFG_SBAS_L1CA = 65536 # When gnssId is 1 (SBAS) # * 0x01 = SBAS L1C/A uint32 SIG_CFG_GALILEO_E1OS = 65536 # When gnssId is 2 (Galileo) - # * 0x01 = Galileo E1OS (not supported in + # * 0x01 = Galileo E1OS (not supported in # protocol versions less than 18) uint32 SIG_CFG_BEIDOU_B1I = 65536 # When gnssId is 3 (BeiDou) - # * 0x01 = BeiDou B1I + # * 0x01 = BeiDou B1I uint32 SIG_CFG_IMES_L1 = 65536 # When gnssId is 4 (IMES) # * 0x01 = IMES L1 uint32 SIG_CFG_QZSS_L1CA = 65536 # When gnssId is 5 (QZSS) # * 0x01 = QZSS L1C/A uint32 SIG_CFG_QZSS_L1SAIF = 262144 # * 0x04 = QZSS L1SAIF uint32 SIG_CFG_GLONASS_L1OF = 65536 # When gnssId is 6 (GLONASS) - # * 0x01 = GLONASS L1OF \ No newline at end of file + # * 0x01 = GLONASS L1OF diff --git a/ublox_msgs/msg/CfgHNR.msg b/ublox_msgs/msg/CfgHNR.msg index e81e8adb..ee6faca6 100644 --- a/ublox_msgs/msg/CfgHNR.msg +++ b/ublox_msgs/msg/CfgHNR.msg @@ -1,11 +1,11 @@ # CFG-HNR (0x06 0x5C) # High Navigation Rate Settings # -# The u-blox receivers support high rates of navigation update up to 30 Hz. +# The u-blox receivers support high rates of navigation update up to 30 Hz. # The navigation solution output (NAV-HNR) will not be aligned to the top of a # second. # The update rate has a direct influence on the power consumption. The more -# fixes that are required, the more CPU power and communication resources are +# fixes that are required, the more CPU power and communication resources are # required. # For most applications a 1 Hz update rate would be sufficient. # @@ -15,5 +15,5 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 92 -uint8 highNavRate # Rate of navigation solution output [Hz] -uint8[3] reserved0 # Reserved \ No newline at end of file +uint8 high_nav_rate # Rate of navigation solution output [Hz] +uint8[3] reserved0 # Reserved diff --git a/ublox_msgs/msg/CfgINF.msg b/ublox_msgs/msg/CfgINF.msg index 8d93dae1..ae11b93d 100644 --- a/ublox_msgs/msg/CfgINF.msg +++ b/ublox_msgs/msg/CfgINF.msg @@ -1,13 +1,13 @@ # CFG-INF (0x06 0x02) # Information message configuration # -# The value of infMsgMask[x] below are that each bit represents one of the INF -# class messages (Bit 0 for ERROR, Bit 1 for WARNING and so on.). For a complete -# list, see the Message Class INF. Several configurations can be concatenated to +# The value of infMsgMask[x] below are that each bit represents one of the INF +# class messages (Bit 0 for ERROR, Bit 1 for WARNING and so on.). For a complete +# list, see the Message Class INF. Several configurations can be concatenated to # one input message. -# In this case the payload length can be a multiple of the normal length. Output -# messages from the module contain only one configuration unit. Note that I/O -# Ports 1 and 2 correspond to serial ports 1 and 2. I/O port 0 is DDC. I/O port +# In this case the payload length can be a multiple of the normal length. Output +# messages from the module contain only one configuration unit. Note that I/O +# Ports 1 and 2 correspond to serial ports 1 and 2. I/O port 0 is DDC. I/O port # 3 is USB. I/O port 4 is SPI. I/O port 5 is reserved for future use # @@ -15,5 +15,5 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 2 # start of repeated block -CfgINF_Block[] blocks +CfgINFBlock[] blocks # end of repeated block diff --git a/ublox_msgs/msg/CfgINFBlock.msg b/ublox_msgs/msg/CfgINFBlock.msg new file mode 100644 index 00000000..d18c6d37 --- /dev/null +++ b/ublox_msgs/msg/CfgINFBlock.msg @@ -0,0 +1,21 @@ +# See CfgINF message +# + +uint8 protocol_id # Protocol Identifier, identifying for which + # protocol the configuration is set/get. The + # following are valid Protocol Identifiers: + # 0: UBX Protocol + # 1: NMEA Protocol + # 2-255: Reserved +uint8 PROTOCOL_ID_UBX = 0 +uint8 PROTOCOL_ID_NMEA = 1 + +uint8[3] reserved1 # Reserved + +uint8[6] inf_msg_mask # A bit mask, saying which information messages + # are enabled on each I/O port +uint8 INF_MSG_ERROR = 1 # enable ERROR +uint8 INF_MSG_WARNING = 2 # enable WARNING +uint8 INF_MSG_NOTICE = 4 # enable NOTICE +uint8 INF_MSG_TEST = 8 # enable TEST +uint8 INF_MSG_DEBUG = 16 # enable DEBUG diff --git a/ublox_msgs/msg/CfgINF_Block.msg b/ublox_msgs/msg/CfgINF_Block.msg deleted file mode 100644 index 4929438c..00000000 --- a/ublox_msgs/msg/CfgINF_Block.msg +++ /dev/null @@ -1,21 +0,0 @@ -# See CfgINF message -# - -uint8 protocolID # Protocol Identifier, identifying for which - # protocol the configuration is set/get. The - # following are valid Protocol Identifiers: - # 0: UBX Protocol - # 1: NMEA Protocol - # 2-255: Reserved -uint8 PROTOCOL_ID_UBX = 0 -uint8 PROTOCOL_ID_NMEA = 1 - -uint8[3] reserved1 # Reserved - -uint8[6] infMsgMask # A bit mask, saying which information messages - # are enabled on each I/O port -uint8 INF_MSG_ERROR = 1 # enable ERROR -uint8 INF_MSG_WARNING = 2 # enable WARNING -uint8 INF_MSG_NOTICE = 4 # enable NOTICE -uint8 INF_MSG_TEST = 8 # enable TEST -uint8 INF_MSG_DEBUG = 16 # enable DEBUG \ No newline at end of file diff --git a/ublox_msgs/msg/CfgMSG.msg b/ublox_msgs/msg/CfgMSG.msg index 64065e8f..d6bc5637 100644 --- a/ublox_msgs/msg/CfgMSG.msg +++ b/ublox_msgs/msg/CfgMSG.msg @@ -1,12 +1,12 @@ # CFG-MSG (0x06 0x01) # Message Rate(s) -# +# # Set message rate for the current port uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 1 -uint8 msgClass # Message Class -uint8 msgID # Message Identifier -uint8 rate # Send rate on current port - # [number of navigation solutions] +uint8 msg_class # Message Class +uint8 msg_id # Message Identifier +uint8 rate # Send rate on current port + # [number of navigation solutions] diff --git a/ublox_msgs/msg/CfgNAV5.msg b/ublox_msgs/msg/CfgNAV5.msg index 0010b6f4..3f044a12 100644 --- a/ublox_msgs/msg/CfgNAV5.msg +++ b/ublox_msgs/msg/CfgNAV5.msg @@ -4,8 +4,8 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 36 -uint16 mask # Parameters Bitmask. Only the masked - # parameters will be applied. +uint16 mask # Parameters Bitmask. Only the masked + # parameters will be applied. uint16 MASK_DYN = 1 # Apply dynamic model settings uint16 MASK_MIN_EL = 2 # Apply minimum elevation settings uint16 MASK_FIX_MODE = 4 # Apply fix mode settings @@ -17,7 +17,7 @@ uint16 MASK_DGPS_MASK = 128 # Apply DGPS settings, firmware >= 7 only uint16 MASK_CNO = 256 # Apply CNO threshold settings uint16 MASK_UTC = 1024 # Apply UTC settings, protocol >= 16 only -uint8 dynModel # Dynamic Platform model: +uint8 dyn_model # Dynamic Platform model: uint8 DYN_MODEL_PORTABLE = 0 # Portable uint8 DYN_MODEL_STATIONARY = 2 # Stationary uint8 DYN_MODEL_PEDESTRIAN = 3 # Pedestrian @@ -28,39 +28,39 @@ uint8 DYN_MODEL_AIRBORNE_2G = 7 # Airborne with <2g Acceleration uint8 DYN_MODEL_AIRBORNE_4G = 8 # Airborne with <4g Acceleration uint8 DYN_MODEL_WRIST_WATCH = 9 # Wrist watch, protocol >= 18 -uint8 fixMode # Position Fixing Mode. +uint8 fix_mode # Position Fixing Mode. uint8 FIX_MODE_2D_ONLY = 1 # 2D only uint8 FIX_MODE_3D_ONLY = 2 # 3D only uint8 FIX_MODE_AUTO = 3 # Auto 2D/3D -int32 fixedAlt # Fixed altitude (mean sea level) for 2D fix mode. - # [m / 0.01] -uint32 fixedAltVar # Fixed altitude variance for 2D mode. [m^2 / 0.0001] -int8 minElev # Minimum Elevation for a GNSS satellite to be used in - # NAV [deg] -uint8 drLimit # Maximum time to perform dead reckoning [s] - # (linear extrapolation) in case of GPS signal loss -uint16 pDop # Position DOP Mask to use [1 / 0.1] -uint16 tDop # Time DOP Mask to use [1 / 0.1] -uint16 pAcc # Position Accuracy Mask [m] -uint16 tAcc # Time Accuracy Mask [m] -uint8 staticHoldThresh # Static hold threshold [cm/s] -uint8 dgnssTimeOut # DGNSS timeout, firmware 7 and newer only [s] +int32 fixed_alt # Fixed altitude (mean sea level) for 2D fix mode. + # [m / 0.01] +uint32 fixed_alt_var # Fixed altitude variance for 2D mode. [m^2 / 0.0001] +int8 min_elev # Minimum Elevation for a GNSS satellite to be used in + # NAV [deg] +uint8 dr_limit # Maximum time to perform dead reckoning [s] + # (linear extrapolation) in case of GPS signal loss +uint16 p_dop # Position DOP Mask to use [1 / 0.1] +uint16 t_dop # Time DOP Mask to use [1 / 0.1] +uint16 p_acc # Position Accuracy Mask [m] +uint16 t_acc # Time Accuracy Mask [m] +uint8 static_hold_thresh # Static hold threshold [cm/s] +uint8 dgnss_time_out # DGNSS timeout, firmware 7 and newer only [s] -uint8 cnoThreshNumSvs # Number of satellites required to have C/N0 above - # cnoThresh for a fix to be attempted -uint8 cnoThresh # C/N0 threshold for deciding whether to attempt a fix - # [dBHz] -uint8[2] reserved1 # Reserved +uint8 cno_thresh_num_svs # Number of satellites required to have C/N0 above + # cnoThresh for a fix to be attempted +uint8 cno_thresh # C/N0 threshold for deciding whether to attempt a fix + # [dBHz] +uint8[2] reserved1 # Reserved -uint16 staticHoldMaxDist # Static hold distance threshold (before quitting - # static hold) [m] -uint8 utcStandard # UTC standard to be used: -uint8 UTC_STANDARD_AUTOMATIC = 0 # receiver selects based on GNSS configuration -uint8 UTC_STANDARD_GPS = 3 # UTC as operated by the U.S. Naval Observatory - # (USNO); derived from GPS time -uint8 UTC_STANDARD_GLONASS = 6 # UTC as operated by the former Soviet Union; - # derived from GLONASS time -uint8 UTC_STANDARD_BEIDOU = 7 # UTC as operated by the National Time Service - # Center, China; derived from BeiDou time -uint8[5] reserved2 # Reserved \ No newline at end of file +uint16 static_hold_max_dist # Static hold distance threshold (before quitting + # static hold) [m] +uint8 utc_standard # UTC standard to be used: +uint8 UTC_STANDARD_AUTOMATIC = 0 # receiver selects based on GNSS configuration +uint8 UTC_STANDARD_GPS = 3 # UTC as operated by the U.S. Naval Observatory + # (USNO); derived from GPS time +uint8 UTC_STANDARD_GLONASS = 6 # UTC as operated by the former Soviet Union; + # derived from GLONASS time +uint8 UTC_STANDARD_BEIDOU = 7 # UTC as operated by the National Time Service + # Center, China; derived from BeiDou time +uint8[5] reserved2 # Reserved diff --git a/ublox_msgs/msg/CfgNAVX5.msg b/ublox_msgs/msg/CfgNAVX5.msg index dde005d9..74ff9225 100644 --- a/ublox_msgs/msg/CfgNAVX5.msg +++ b/ublox_msgs/msg/CfgNAVX5.msg @@ -6,62 +6,62 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 35 -uint16 version # Message version (set to 0) +uint16 version # Message version (set to 0) -uint16 mask1 # First parameters bitmask (possible values below) +uint16 mask1 # First parameters bitmask (possible values below) uint16 MASK1_MIN_MAX = 4 # apply min/max SVs settings uint16 MASK1_MIN_CNO = 8 # apply minimum C/N0 setting uint16 MASK1_INITIAL_FIX_3D = 64 # apply initial 3D fix settings uint16 MASK1_WKN_ROLL = 512 # apply GPS week number rollover settings -uint16 MASK1_ACK_AID = 1024 # apply assistance acknowledgment +uint16 MASK1_ACK_AID = 1024 # apply assistance acknowledgment # settings uint16 MASK1_PPP = 8192 # apply usePPP flag -uint16 MASK1_AOP = 16384 # apply aopCfg (useAOP flag) and +uint16 MASK1_AOP = 16384 # apply aopCfg (useAOP flag) and # aopOrbMaxErr settings # (AssistNow Autonomous) -uint32 mask2 # Second parameters bitmask (possible values below) - # Firmware >=8 only -uint32 MASK2_ADR = 64 # Apply ADR sensor fusion on/off - # setting -uint32 MASK2_SIG_ATTEN_COMP_MODE = 128 # Apply signal attenuation - # compensation feature settings +uint32 mask2 # Second parameters bitmask (possible values below) + # Firmware >=8 only +uint32 MASK2_ADR = 64 # Apply ADR sensor fusion on/off + # setting +uint32 MASK2_SIG_ATTEN_COMP_MODE = 128 # Apply signal attenuation + # compensation feature settings -uint8[2] reserved1 # Always set to zero +uint8[2] reserved1 # Always set to zero -uint8 minSVs # Minimum number of satellites for navigation -uint8 maxSVs # Maximum number of satellites for navigation -uint8 minCNO # Minimum satellite signal level for navigation [dBHz] +uint8 min_svs # Minimum number of satellites for navigation +uint8 max_svs # Maximum number of satellites for navigation +uint8 min_cno # Minimum satellite signal level for navigation [dBHz] -uint8 reserved2 # Always set to zero +uint8 reserved2 # Always set to zero -uint8 iniFix3D # If set to 1, initial fix must be 3D +uint8 ini_fix3d # If set to 1, initial fix must be 3D -uint8[2] reserved3 # Always set to zero +uint8[2] reserved3 # Always set to zero -uint8 ackAiding # If set to 1, issue acknowledgments for assistance -uint16 wknRollover # GPS week rollover number, GPS week numbers will be set - # correctly from this week up to 1024 weeks after this - # week -uint8 sigAttenCompMode # Permanently attenuated signal compensation [dBHz] - # 0 = disabled, 255 = automatic - # 1..63 = maximum expected C/N0 value - # Firmware 8 only +uint8 ack_aiding # If set to 1, issue acknowledgments for assistance +uint16 wkn_rollover # GPS week rollover number, GPS week numbers will be set + # correctly from this week up to 1024 weeks after this + # week +uint8 sig_atten_comp_mode # Permanently attenuated signal compensation [dBHz] + # 0 = disabled, 255 = automatic + # 1..63 = maximum expected C/N0 value + # Firmware 8 only -uint8[5] reserved4 # Always set to zero +uint8[5] reserved4 # Always set to zero -uint8 usePPP # Enable/disable PPP (on supported units) -uint8 aopCfg # AssistNow Autonomous configuration, 1: enabled +uint8 use_ppp # Enable/disable PPP (on supported units) +uint8 aop_cfg # AssistNow Autonomous configuration, 1: enabled -uint8[2] reserved5 # Always set to zero +uint8[2] reserved5 # Always set to zero -uint16 aopOrbMaxErr # Maximum acceptable (modeled) autonomous orbit - # error [m] - # valid range = 5..1000 - # 0 = reset to firmware default +uint16 aop_orb_max_err # Maximum acceptable (modeled) autonomous orbit + # error [m] + # valid range = 5..1000 + # 0 = reset to firmware default -uint8[7] reserved6 # Always set to zero +uint8[7] reserved6 # Always set to zero -uint8 useAdr # Enable/disable ADR sensor fusion - # 1: enabled, 0: disabled - # Only supported on certain products +uint8 use_adr # Enable/disable ADR sensor fusion + # 1: enabled, 0: disabled + # Only supported on certain products diff --git a/ublox_msgs/msg/CfgNMEA.msg b/ublox_msgs/msg/CfgNMEA.msg index e029e5c5..e3486e15 100644 --- a/ublox_msgs/msg/CfgNMEA.msg +++ b/ublox_msgs/msg/CfgNMEA.msg @@ -1,16 +1,16 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration V1 # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags -uint8 FILTER_POS = 1 # Enable position output for failed or +uint8 FILTER_POS = 1 # Enable position output for failed or # invalid fixes uint8 FILTER_MSK_POS = 2 # Enable position output for invalid fixes uint8 FILTER_TIME = 4 # Enable time output for invalid times @@ -18,30 +18,30 @@ uint8 FILTER_DATE = 8 # Enable date output for invalid dates uint8 FILTER_GPS_ONLY = 16 # Restrict output to GPS satellites only uint8 FILTER_TRACK = 32 # Enable COG output even if COG is frozen -uint8 nmeaVersion # NMEA version +uint8 nmea_version # NMEA version uint8 NMEA_VERSION_4_1 = 65 # Version 4.1 uint8 NMEA_VERSION_4_0 = 64 # Version 4.0 uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report per TalkerId: +uint8 num_sv # Maximum Number of SVs to report per TalkerId: # unlimited (0) or 8, 12, 16 -uint8 NUM_SV_UNLIMITED = 0 +uint8 NUM_SV_UNLIMITED = 0 uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode uint8 FLAGS_LIMIT82 = 4 # enable strict limit to 82 characters maximum uint8 FLAGS_HIGH_PREC = 8 # enable high precision mode - # This flag cannot be set in conjunction with + # This flag cannot be set in conjunction with # either Compatibility Mode or Limit82 Mode. # (not supported in protocol versions < 20.01) -uint32 gnssToFilter # Filters out satellites based on their GNSS. - # If a bitfield is enabled, the corresponding +uint32 gnss_to_filter # Filters out satellites based on their GNSS. + # If a bitfield is enabled, the corresponding # satellites will be not output. uint32 GNSS_TO_FILTER_GPS = 1 # Disable reporting of GPS satellites uint32 GNSS_TO_FILTER_SBAS = 2 # Disable reporting of SBAS satellites @@ -49,17 +49,17 @@ uint32 GNSS_TO_FILTER_QZSS = 16 # Disable reporting of QZSS satellites uint32 GNSS_TO_FILTER_GLONASS = 32 # Disable reporting of GLONASS satellites uint32 GNSS_TO_FILTER_BEIDOU = 64 # Disable reporting of BeiDou satellites -uint8 svNumbering # Configures the display of satellites that do not +uint8 sv_numbering # Configures the display of satellites that do not # have an NMEA-defined value. Note: this does not # apply to satellites with an unknown ID. uint8 SV_NUMBERING_STRICT = 0 # Strict - Satellites are not output uint8 SV_NUMBERING_EXTENDED = 1 # Extended - Use proprietary numbering -uint8 mainTalkerId # By default the main Talker ID (i.e. the Talker - # ID used for all messages other than GSV) is - # determined by the GNSS assignment of the - # receiver's channels (see CfgGNSS). - # This field enables the main Talker ID to be +uint8 main_talker_id # By default the main Talker ID (i.e. the Talker + # ID used for all messages other than GSV) is + # determined by the GNSS assignment of the + # receiver's channels (see CfgGNSS). + # This field enables the main Talker ID to be # overridden uint8 MAIN_TALKER_ID_NOT_OVERRIDDEN = 0 # Main Talker ID is not overridden uint8 MAIN_TALKER_ID_GP = 1 # Set main Talker ID to 'GP' @@ -68,18 +68,18 @@ uint8 MAIN_TALKER_ID_GN = 3 # Set main Talker ID to 'GN' uint8 MAIN_TALKER_ID_GA = 4 # Set main Talker ID to 'GA' uint8 MAIN_TALKER_ID_GB = 5 # Set main Talker ID to 'GB' -uint8 gsvTalkerId # By default the Talker ID for GSV messages is - # GNSS specific (as defined by NMEA). This field +uint8 gsv_talker_id # By default the Talker ID for GSV messages is + # GNSS specific (as defined by NMEA). This field # enables the GSV Talker ID to be overridden. -uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID +uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID # (as defined by NMEA) uint8 GSV_TALKER_ID_MAIN = 1 # Use the main Talker ID uint8 version # Message version (set to 1 for this version) uint8 VERSION = 1 -uint8[2] bdsTalkerId # Sets the two characters that should be used - # for the BeiDou Talker ID. If these are set to +uint8[2] bds_talker_id # Sets the two characters that should be used + # for the BeiDou Talker ID. If these are set to # zero, the default BeiDou TalkerId will be used -uint8[6] reserved1 # Reserved \ No newline at end of file +uint8[6] reserved1 # Reserved diff --git a/ublox_msgs/msg/CfgNMEA6.msg b/ublox_msgs/msg/CfgNMEA6.msg index a62cc358..94e83e06 100644 --- a/ublox_msgs/msg/CfgNMEA6.msg +++ b/ublox_msgs/msg/CfgNMEA6.msg @@ -1,15 +1,15 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # # Supported on u-blox 6 from firmware version 6.00 up to version 7.03. # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags uint8 FILTER_POS = 1 # Disable position filtering @@ -23,7 +23,7 @@ uint8 version # NMEA version uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report in NMEA +uint8 num_sv # Maximum Number of SVs to report in NMEA # protocol. # This does not affect the receiver's operation. # It only limits the number of SVs reported in @@ -33,7 +33,7 @@ uint8 numSV # Maximum Number of SVs to report in NMEA uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode diff --git a/ublox_msgs/msg/CfgNMEA7.msg b/ublox_msgs/msg/CfgNMEA7.msg index ffb094f9..7a00e99f 100644 --- a/ublox_msgs/msg/CfgNMEA7.msg +++ b/ublox_msgs/msg/CfgNMEA7.msg @@ -1,18 +1,18 @@ # CFG-NMEA (0x06 0x17) # NMEA protocol configuration V0 # -# Set/Get the NMEA protocol configuration. See section NMEA Protocol -# Configuration for a detailed description of the configuration effects on +# Set/Get the NMEA protocol configuration. See section NMEA Protocol +# Configuration for a detailed description of the configuration effects on # NMEA output # # Supported on: u-blox 7 firmware version 1.00 # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 23 +uint8 MESSAGE_ID = 23 uint8 filter # filter flags -uint8 FILTER_POS = 1 # Enable position output for failed or +uint8 FILTER_POS = 1 # Enable position output for failed or # invalid fixes uint8 FILTER_MSK_POS = 2 # Enable position output for invalid fixes uint8 FILTER_TIME = 4 # Enable time output for invalid times @@ -20,51 +20,51 @@ uint8 FILTER_DATE = 8 # Enable date output for invalid dates uint8 FILTER_GPS_ONLY = 16 # Restrict output to GPS satellites only uint8 FILTER_TRACK = 32 # Enable COG output even if COG is frozen -uint8 nmeaVersion # NMEA version +uint8 nmea_version # NMEA version uint8 NMEA_VERSION_2_3 = 35 # Version 2.3 uint8 NMEA_VERSION_2_1 = 33 # Version 2.1 -uint8 numSV # Maximum Number of SVs to report per TalkerId: +uint8 num_sv # Maximum Number of SVs to report per TalkerId: # unlimited (0) or 8, 12, 16 -uint8 NUM_SV_UNLIMITED = 0 +uint8 NUM_SV_UNLIMITED = 0 uint8 flags # flags uint8 FLAGS_COMPAT = 1 # enable compatibility mode. - # This might be needed for certain applications - # when customer's NMEA parser expects a fixed + # This might be needed for certain applications + # when customer's NMEA parser expects a fixed # number of digits in position coordinates uint8 FLAGS_CONSIDER = 2 # enable considering mode -uint32 gnssToFilter # Filters out satellites based on their GNSS. - # If a bitfield is enabled, the corresponding +uint32 gnss_to_filter # Filters out satellites based on their GNSS. + # If a bitfield is enabled, the corresponding # satellites will be not output. uint32 GNSS_TO_FILTER_GPS = 1 # Disable reporting of GPS satellites uint32 GNSS_TO_FILTER_SBAS = 2 # Disable reporting of SBAS satellites uint32 GNSS_TO_FILTER_QZSS = 16 # Disable reporting of QZSS satellites uint32 GNSS_TO_FILTER_GLONASS = 32 # Disable reporting of GLONASS satellites -uint8 svNumbering # Configures the display of satellites that do not +uint8 sv_numbering # Configures the display of satellites that do not # have an NMEA-defined value. Note: this does not # apply to satellites with an unknown ID. uint8 SV_NUMBERING_STRICT = 0 # Strict - Satellites are not output uint8 SV_NUMBERING_EXTENDED = 1 # Extended - Use proprietary numbering -uint8 mainTalkerId # By default the main Talker ID (i.e. the Talker - # ID used for all messages other than GSV) is - # determined by the GNSS assignment of the - # receiver's channels (see CfgGNSS). - # This field enables the main Talker ID to be +uint8 main_talker_id # By default the main Talker ID (i.e. the Talker + # ID used for all messages other than GSV) is + # determined by the GNSS assignment of the + # receiver's channels (see CfgGNSS). + # This field enables the main Talker ID to be # overridden uint8 MAIN_TALKER_ID_NOT_OVERRIDDEN = 0 # Main Talker ID is not overridden uint8 MAIN_TALKER_ID_GP = 1 # Set main Talker ID to 'GP' uint8 MAIN_TALKER_ID_GL = 2 # Set main Talker ID to 'GL' uint8 MAIN_TALKER_ID_GN = 3 # Set main Talker ID to 'GN' -uint8 gsvTalkerId # By default the Talker ID for GSV messages is - # GNSS specific (as defined by NMEA). This field +uint8 gsv_talker_id # By default the Talker ID for GSV messages is + # GNSS specific (as defined by NMEA). This field # enables the GSV Talker ID to be overridden. -uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID +uint8 GSV_TALKER_ID_GNSS_SPECIFIC = 0 # Use GNSS specific Talker ID # (as defined by NMEA) uint8 GSV_TALKER_ID_MAIN = 1 # Use the main Talker ID -uint8 reserved # Reserved \ No newline at end of file +uint8 reserved # Reserved diff --git a/ublox_msgs/msg/CfgPRT.msg b/ublox_msgs/msg/CfgPRT.msg index 97580cb3..cb0c0903 100644 --- a/ublox_msgs/msg/CfgPRT.msg +++ b/ublox_msgs/msg/CfgPRT.msg @@ -1,7 +1,7 @@ # CFG-PRT (0x06 0x00) # Port Configuration for DDC, UART, USB, SPI # -# Several configurations can be concatenated to one input message. In this case +# Several configurations can be concatenated to one input message. In this case # the payload length can be a multiple of the normal length (see the other # versions of CFG-PRT). Output messages from the module contain only one # configuration unit. @@ -10,7 +10,7 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 0 -uint8 portID # Port Identifier Number +uint8 port_id # Port Identifier Number uint8 PORT_ID_DDC = 0 uint8 PORT_ID_UART1 = 1 uint8 PORT_ID_UART2 = 2 @@ -19,10 +19,10 @@ uint8 PORT_ID_SPI = 4 uint8 reserved0 # Reserved -uint16 txReady # TX ready PIN configuration (since Firmware 7.01) +uint16 tx_ready # TX ready PIN configuration (since Firmware 7.01) # reserved (Always set to zero) up to Firmware 7.01 - -uint16 TX_READY_EN = 1 # Enable TX ready feature for this + +uint16 TX_READY_EN = 1 # Enable TX ready feature for this # port uint16 TX_READY_POLARITY_HIGH_ACTIVE = 0 # Polarity High-active uint16 TX_READY_POLARITY_LOW_ACTIVE = 2 # Polarity Low-active @@ -32,19 +32,19 @@ uint16 TX_READY_PIN_MASK = 124 # uint16 TX_READY_THRES_SHIFT = 7 # Threshold uint16 TX_READY_THRES_MASK = 65408 # The given threshold is multiplied by # 8 bytes. - # The TX ready PIN goes active + # The TX ready PIN goes active # after >= thres*8 bytes are pending # for the port and going inactive # after the last pending bytes have # been written to hardware (0-4 bytes # before end of stream). -uint32 mode # A bit mask describing the DDC, UART or SPI mode +uint32 mode # A bit mask describing the DDC, UART or SPI mode # Reserved for USB # DDC Mode Constants -uint32 MODE_DDC_SLAVE_ADDR_SHIFT = 1 +uint32 MODE_DDC_SLAVE_ADDR_SHIFT = 1 uint32 MODE_DDC_SLAVE_ADDR_MASK = 254 # Slave address - # Range: 0x07 < slaveAddr < 0x78. + # Range: 0x07 < slaveAddr < 0x78. # UART Mode Constants uint32 MODE_RESERVED1 = 16 # Default 1 for compatibility with A4 uint32 MODE_CHAR_LEN_MASK = 192 # Character Length @@ -70,21 +70,21 @@ uint32 MODE_SPI_FLOW_CONTROL = 64 # (u-blox 6 only) # 1 Flow control enabled (9-bit mode) uint32 MODE_SPI_FF_COUNT_SHIFT = 8 uint32 MODE_SPI_FF_COUNT_MASK = 16128 # Number of bytes containing 0xFF to - # receive before switching off - # reception. + # receive before switching off + # reception. # Range: 0(mechanism off)-63 -uint32 baudRate # UART Baudrate in bits/second [bits/s] +uint32 baud_rate # UART Baudrate in bits/second [bits/s] # Reserved for USB, SPI, DDC - # Possible values: 4800, 9600, 19200, 38400, 57600, + # Possible values: 4800, 9600, 19200, 38400, 57600, # 115200, 230400, 460800 -uint16 inProtoMask # A mask describing which input protocols are active. +uint16 in_proto_mask # A mask describing which input protocols are active. # Each bit of this mask is used for a protocol. # Through that, multiple protocols can be defined # on a single port. -uint16 outProtoMask # A mask describing which output protocols are active. +uint16 out_proto_mask # A mask describing which output protocols are active. # Each bit of this mask is used for a protocol. # Through that, multiple protocols can be defined # on a single port. @@ -95,11 +95,11 @@ uint16 PROTO_RTCM = 4 # only for inProtoMask uint16 PROTO_RTCM3 = 32 # (not supported in protocol versions less than 20) uint16 flags # Flags for UART & SPI, Reserved for USB -uint16 FLAGS_EXTENDED_TX_TIMEOUT = 2 # if set, the port will timeout if - # allocated TX memory >=4 kB and no - # activity for 1.5s. If not set the port - # will timoout if no activity for 1.5s - # regardless on the amount of allocated +uint16 FLAGS_EXTENDED_TX_TIMEOUT = 2 # if set, the port will timeout if + # allocated TX memory >=4 kB and no + # activity for 1.5s. If not set the port + # will timoout if no activity for 1.5s + # regardless on the amount of allocated # TX memory. uint16 reserved1 # Always set to zero diff --git a/ublox_msgs/msg/CfgRATE.msg b/ublox_msgs/msg/CfgRATE.msg index f8beda7b..23b7e55d 100644 --- a/ublox_msgs/msg/CfgRATE.msg +++ b/ublox_msgs/msg/CfgRATE.msg @@ -1,17 +1,17 @@ # CFG-RATE (0x06 0x08) # Navigation/Measurement Rate Settings # -# This message allows the user to alter the rate at which navigation solutions -# (and the measurements that they depend on) are generated by the receiver. The -# calculation of the navigation solution will always be aligned to the top of a -# second zero (first second of the week) of the configured reference time -# system. For protocol version 18 and later the navigation period is an integer +# This message allows the user to alter the rate at which navigation solutions +# (and the measurements that they depend on) are generated by the receiver. The +# calculation of the navigation solution will always be aligned to the top of a +# second zero (first second of the week) of the configured reference time +# system. For protocol version 18 and later the navigation period is an integer # multiple of the measurement period. # - Each measurement triggers the measurements generation and raw data output. -# - The navRate value defines that every nth measurement triggers a navigation +# - The navRate value defines that every nth measurement triggers a navigation # epoch. -# - The update rate has a direct influence on the power consumption. The more -# fixes that are required, the more CPU power and communication resources +# - The update rate has a direct influence on the power consumption. The more +# fixes that are required, the more CPU power and communication resources # are required. # - For most applications a 1 Hz update rate would be sufficient. # - When using Power Save Mode, measurement and navigation rate can differ from @@ -20,12 +20,12 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 8 -uint16 measRate # Measurement Rate, GPS measurements are +uint16 meas_rate # Measurement Rate, GPS measurements are # taken every measRate milliseconds [ms] -uint16 navRate # Navigation Rate, in number of measurement +uint16 nav_rate # Navigation Rate, in number of measurement # cycles. On u-blox 5 and u-blox 6, this parameter # cannot be changed, and always equals 1. -uint16 timeRef # The time system to which measurements are aligned +uint16 time_ref # The time system to which measurements are aligned uint16 TIME_REF_UTC = 0 uint16 TIME_REF_GPS = 1 uint16 TIME_REF_GLONASS = 2 # not supported in protocol versions less than 18 diff --git a/ublox_msgs/msg/CfgRST.msg b/ublox_msgs/msg/CfgRST.msg index 76844e88..2d5683ee 100644 --- a/ublox_msgs/msg/CfgRST.msg +++ b/ublox_msgs/msg/CfgRST.msg @@ -8,9 +8,9 @@ # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 4 +uint8 MESSAGE_ID = 4 -uint16 navBbrMask # BBR Sections to clear. +uint16 nav_bbr_mask # BBR Sections to clear. # The following Special Sets apply: uint16 NAV_BBR_HOT_START = 0 # Hot start the device uint16 NAV_BBR_WARM_START = 1 # Warm start the device @@ -26,13 +26,13 @@ uint16 NAV_BBR_UTC = 128 # UTC Correction + GPS Leap Seconds Parameters uint16 NAV_BBR_RTC = 256 # RTC uint16 NAV_BBR_AOP = 32768 # Autonomous Orbit Parameters -uint8 resetMode # Reset Type: +uint8 reset_mode # Reset Type: uint8 RESET_MODE_HW_IMMEDIATE = 0 # Hardware reset (Watchdog) immediately uint8 RESET_MODE_SW = 1 # Controlled Software reset uint8 RESET_MODE_GNSS = 2 # Controlled Software reset (GNSS only) -uint8 RESET_MODE_HW_AFTER_SHUTDOWN = 4 # Hardware reset (Watchdog) after +uint8 RESET_MODE_HW_AFTER_SHUTDOWN = 4 # Hardware reset (Watchdog) after # shutdown uint8 RESET_MODE_GNSS_STOP = 8 # Controlled GNSS stop uint8 RESET_MODE_GNSS_START = 9 # Controlled GNSS start -uint8 reserved1 # Reserved \ No newline at end of file +uint8 reserved1 # Reserved diff --git a/ublox_msgs/msg/CfgSBAS.msg b/ublox_msgs/msg/CfgSBAS.msg index 0211d3fc..a46fe1ff 100644 --- a/ublox_msgs/msg/CfgSBAS.msg +++ b/ublox_msgs/msg/CfgSBAS.msg @@ -11,9 +11,9 @@ uint8 MESSAGE_ID = 22 uint8 mode # SBAS Mode uint8 MODE_ENABLED = 1 # SBAS Enabled (1) / Disabled (0) - # This field is deprecated; use UBX-CFG-GNSS to + # This field is deprecated; use UBX-CFG-GNSS to # enable/disable SBAS operation -uint8 MODE_TEST = 2 # SBAS Testbed: Use data anyhow (1) / Ignore data when +uint8 MODE_TEST = 2 # SBAS Testbed: Use data anyhow (1) / Ignore data when # in Test Mode (SBAS Msg 0) uint8 usage # SBAS Usage @@ -21,7 +21,7 @@ uint8 USAGE_RANGE = 1 # Use SBAS GEOs as a ranging source (for navigation) uint8 USAGE_DIFF_CORR = 2 # Use SBAS Differential Corrections uint8 USAGE_INTEGRITY = 4 # Use SBAS Integrity Information -uint8 maxSBAS # Maximum Number of SBAS prioritized tracking +uint8 max_sbas # Maximum Number of SBAS prioritized tracking # channels (valid range: 0 - 3) to use # (obsolete and superseeded by UBX-CFG-GNSS in protocol # versions 14+). @@ -31,6 +31,6 @@ uint8 scanmode2 # Continuation of scanmode bitmask below # PRN 152...158 uint32 scanmode1 # Which SBAS PRN numbers to search for (Bitmask) # If all Bits are set to zero, auto-scan (i.e. all valid - # PRNs) are searched. Every bit corresponds to a PRN + # PRNs) are searched. Every bit corresponds to a PRN # number. # PRN 120..151 diff --git a/ublox_msgs/msg/CfgTMODE3.msg b/ublox_msgs/msg/CfgTMODE3.msg index a4954439..9c31cb62 100644 --- a/ublox_msgs/msg/CfgTMODE3.msg +++ b/ublox_msgs/msg/CfgTMODE3.msg @@ -2,9 +2,9 @@ # Time Mode Settings 3 # # Configures the receiver to be in Time Mode. The position referred to in this -# message is that of the Antenna Reference Point (ARP). See the Time Mode +# message is that of the Antenna Reference Point (ARP). See the Time Mode # Description for details. -# +# # Supported on: # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision # GNSS products) @@ -21,19 +21,19 @@ uint16 FLAGS_MODE_MASK = 255 # Receiver Mode: uint16 FLAGS_MODE_DISABLED = 0 # Disabled uint16 FLAGS_MODE_SURVEY_IN = 1 # Survey In uint16 FLAGS_MODE_FIXED = 2 # Fixed Mode (true ARP position required) -uint16 FLAGS_LLA = 256 # Position is given in LAT/LON/ALT +uint16 FLAGS_LLA = 256 # Position is given in LAT/LON/ALT # (default is ECEF) -int32 ecefXOrLat # WGS84 ECEF X coordinate (or latitude) of +int32 ecef_x_or_lat # WGS84 ECEF X coordinate (or latitude) of # the ARP position, depending on flags above # [cm] or [deg / 1e-7] -int32 ecefYOrLon # WGS84 ECEF Y coordinate (or longitude) of +int32 ecef_y_or_lon # WGS84 ECEF Y coordinate (or longitude) of # the ARP position, depending on flags above # [cm] or [deg / 1e-7] -int32 ecefZOrAlt # WGS84 ECEF Z coordinate (or altitude) of +int32 ecef_z_or_alt # WGS84 ECEF Z coordinate (or altitude) of # the ARP position, depending on flags above # [cm] -int8 ecefXOrLatHP # High-precision WGS84 ECEF X coordinate (or +int8 ecef_x_or_lat_hp # High-precision WGS84 ECEF X coordinate (or # latitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF X coordinate in units @@ -41,15 +41,15 @@ int8 ecefXOrLatHP # High-precision WGS84 ECEF X coordinate (or # units of 1e-7 degrees, is given by # ecefXOrLat + (ecefXOrLatHP * 1e-2) # [0.1 mm] or [deg * 1e-9] -int8 ecefYOrLonHP # High-precision WGS84 ECEF Y coordinate (or +int8 ecef_y_or_lon_hp # High-precision WGS84 ECEF Y coordinate (or # longitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF Y coordinate in units - # of cm, or the precise WGS84 ECEF longitude + # of cm, or the precise WGS84 ECEF longitude # in units of 1e-7 degrees, is given by # ecefYOrLon + (ecefYOrLonHP * 1e-2) # [0.1 mm] or [deg * 1e-9] -int8 ecefZOrAltHP # High-precision WGS84 ECEF Z coordinate (or +int8 ecef_z_or_alt_hp # High-precision WGS84 ECEF Z coordinate (or # altitude) of the ARP position, depending on # flags above. Must be in the range -99..+99. # The precise WGS84 ECEF Z coordinate, or @@ -58,11 +58,11 @@ int8 ecefZOrAltHP # High-precision WGS84 ECEF Z coordinate (or # [0.1 mm] uint8 reserved2 # Reserved -uint32 fixedPosAcc # Fixed position 3D accuracy +uint32 fixed_pos_acc # Fixed position 3D accuracy # [0.1 mm] -uint32 svinMinDur # Survey-in minimum duration +uint32 svin_min_dur # Survey-in minimum duration # [s] -uint32 svinAccLimit # Survey-in position accuracy limit +uint32 svin_acc_limit # Survey-in position accuracy limit # [0.1 mm] uint8[8] reserved3 # Reserved diff --git a/ublox_msgs/msg/CfgUSB.msg b/ublox_msgs/msg/CfgUSB.msg index 2e812d14..c44249b0 100644 --- a/ublox_msgs/msg/CfgUSB.msg +++ b/ublox_msgs/msg/CfgUSB.msg @@ -3,28 +3,28 @@ # uint8 CLASS_ID = 6 -uint8 MESSAGE_ID = 27 +uint8 MESSAGE_ID = 27 -uint16 vendorID # Only set to registered Vendor IDs. +uint16 vendor_id # Only set to registered Vendor IDs. # Changing this field requires special Host drivers. -uint16 productID # Product ID. Changing this field requires special +uint16 product_id # Product ID. Changing this field requires special # Host drivers. uint8[2] reserved1 # Reserved uint8[2] reserved2 # Reserved -uint16 powerConsumption # Power consumed by the device [mA] +uint16 power_consumption # Power consumed by the device [mA] uint16 flags # various configuration flags (see graphic below) uint16 FLAGS_RE_ENUM = 0 # force re-enumeration uint16 FLAGS_POWER_MODE = 2 # self-powered (1), bus-powered (0) -int8[32] vendorString # String containing the vendor name. +int8[32] vendor_string # String containing the vendor name. # 32 ASCII bytes including 0-termination. -int8[32] productString # String containing the product name. +int8[32] product_string # String containing the product name. # 32 ASCII bytes including 0-termination. -int8[32] serialNumber # String containing the serial number. - # 32 ASCII bytes including 0-termination. - # Changing the String fields requires special Host - # drivers. \ No newline at end of file +int8[32] serial_number # String containing the serial number. + # 32 ASCII bytes including 0-termination. + # Changing the String fields requires special Host + # drivers. diff --git a/ublox_msgs/msg/EsfINS.msg b/ublox_msgs/msg/EsfINS.msg index 423c076d..706ae1a8 100644 --- a/ublox_msgs/msg/EsfINS.msg +++ b/ublox_msgs/msg/EsfINS.msg @@ -1,11 +1,11 @@ # ESF-INS (0x10 0x15) # Vehicle dynamics information -# -# This message outputs information about vehicle dynamics computed by the +# +# This message outputs information about vehicle dynamics computed by the # Inertial Navigation System (INS) during ESF-based navigation. -# For ADR products, the output dynamics information (angular rates and +# For ADR products, the output dynamics information (angular rates and # accelerations) is expressed with respect to the vehicle-frame. -# For UDR products, the output dynamics information (angular rates and +# For UDR products, the output dynamics information (angular rates and # accelerations) is expressed with respect to the body-frame. # @@ -14,25 +14,25 @@ uint8 MESSAGE_ID = 21 uint32 bitfield0 # Bitfield (see graphic below) uint32 BITFIELD0_VERSION = 255 # Message version (1 for this version). -uint32 BITFIELD0_X_ANG_RATE_VALID = 256 # Compensated x-axis angular rate data +uint32 BITFIELD0_X_ANG_RATE_VALID = 256 # Compensated x-axis angular rate data # validity flag -uint32 BITFIELD0_Y_ANG_RATE_VALID = 512 # Compensated y-axis angular rate data +uint32 BITFIELD0_Y_ANG_RATE_VALID = 512 # Compensated y-axis angular rate data # validity flag -uint32 BITFIELD0_Z_ANG_RATE_VALID = 1024 # Compensated z-axis angular rate data +uint32 BITFIELD0_Z_ANG_RATE_VALID = 1024 # Compensated z-axis angular rate data # validity flag -uint32 BITFIELD0_X_ACCEL_VALID = 2048 # Compensated x-axis acceleration data +uint32 BITFIELD0_X_ACCEL_VALID = 2048 # Compensated x-axis acceleration data # validity flag -uint32 BITFIELD0_Y_ACCEL_VALID = 4096 # Compensated y-axis acceleration data +uint32 BITFIELD0_Y_ACCEL_VALID = 4096 # Compensated y-axis acceleration data # validity flag -uint32 BITFIELD0_Z_ACCEL_VALID = 8192 # Compensated z-axis acceleration data +uint32 BITFIELD0_Z_ACCEL_VALID = 8192 # Compensated z-axis acceleration data # validity flag uint8[4] reserved1 # Reserved -uint32 iTOW # GPS time of week of the navigation epoch [ms] -int32 xAngRate # Compensated x-axis angular rate [deg/s / 1e-3] -int32 yAngRate # Compensated y-axis angular rate [deg/s / 1e-3] -int32 zAngRate # Compensated z-axis angular rate [deg/s / 1e-3] -int32 xAccel # Compensated x-axis acceleration (gravity-free) [mg] -int32 yAccel # Compensated y-axis acceleration (gravity-free) [mg] -int32 zAccel # Compensated z-axis acceleration (gravity-free) [mg] \ No newline at end of file +uint32 i_tow # GPS time of week of the navigation epoch [ms] +int32 x_ang_rate # Compensated x-axis angular rate [deg/s / 1e-3] +int32 y_ang_rate # Compensated y-axis angular rate [deg/s / 1e-3] +int32 z_ang_rate # Compensated z-axis angular rate [deg/s / 1e-3] +int32 x_accel # Compensated x-axis acceleration (gravity-free) [mg] +int32 y_accel # Compensated y-axis acceleration (gravity-free) [mg] +int32 z_accel # Compensated z-axis acceleration (gravity-free) [mg] diff --git a/ublox_msgs/msg/EsfMEAS.msg b/ublox_msgs/msg/EsfMEAS.msg index e0ab0ffd..24017c76 100644 --- a/ublox_msgs/msg/EsfMEAS.msg +++ b/ublox_msgs/msg/EsfMEAS.msg @@ -1,25 +1,25 @@ # ESF-MEAS (0x10 0x02) # External Sensor Fusion Measurements -# -# Possible data types for the data field are described in the ESF Measurement +# +# Possible data types for the data field are described in the ESF Measurement # Data section # uint8 CLASS_ID = 16 uint8 MESSAGE_ID = 2 -uint32 timeTag # Time tag of measurement generated by external +uint32 time_tag # Time tag of measurement generated by external # sensor uint16 flags # Flags. Set all unused bits to zero: -uint16 FLAGS_TIME_MARK_SENT_MASK = 3 # Time mark signal was supplied just +uint16 FLAGS_TIME_MARK_SENT_MASK = 3 # Time mark signal was supplied just # prior to sending this message: 0 = uint16 TIME_MARK_NONE = 0 uint16 TIME_MARK_EXT0 = 1 uint16 TIME_MARK_EXT = 2 -uint16 FLAGS_TIME_MARK_EDGE = 4 # Trigger on rising (0) or falling +uint16 FLAGS_TIME_MARK_EDGE = 4 # Trigger on rising (0) or falling # (1) edge of time mark signal -uint16 FLAGS_CALIB_T_TAG_VALID = 8 # Calibration time tag available. +uint16 FLAGS_CALIB_T_TAG_VALID = 8 # Calibration time tag available. # Always set to zero. uint16 id # Identification number of data provider @@ -30,40 +30,40 @@ uint32 DATA_FIELD_MASK = 16777215 # data uint32 DATA_TYPE_MASK = 1056964608 # type of data (1..63) uint32 DATA_TYPE_SHIFT = 24 uint32 DATA_TYPE_NONE = 0 # data field contains no data -uint32 DATA_TYPE_Z_AXIS_GYRO = 5 # z-axis gyroscope angular rate +uint32 DATA_TYPE_Z_AXIS_GYRO = 5 # z-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_WHEEL_TICKS_FRONT_LEFT = 6 # front-left wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_FRONT_LEFT = 6 # front-left wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT = 7 # front-right wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT = 7 # front-right wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_REAR_LEFT = 8 # rear-left wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_REAR_LEFT = 8 # rear-left wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_WHEEL_TICKS_REAR_RIGHT = 9 # rear-right wheel ticks - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_WHEEL_TICKS_REAR_RIGHT = 9 # rear-right wheel ticks + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) -uint32 DATA_TYPE_SINGLE_TICK = 10 # single tick (speed tick) - # Bits 0-22: unsigned tick value. - # Bit 23: direction indicator +uint32 DATA_TYPE_SINGLE_TICK = 10 # single tick (speed tick) + # Bits 0-22: unsigned tick value. + # Bit 23: direction indicator # (0=forward, 1=backward) uint32 DATA_TYPE_SPEED = 11 # speed m/s * 1e-3 signed -uint32 DATA_TYPE_GYRO_TEMPERATURE = 12 # gyroscope temperature +uint32 DATA_TYPE_GYRO_TEMPERATURE = 12 # gyroscope temperature # [deg Celsius * 1e-2 signed] -uint32 DATA_TYPE_GYRO_ANG_RATE_Y = 13 # y-axis gyroscope angular rate +uint32 DATA_TYPE_GYRO_ANG_RATE_Y = 13 # y-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_GYRO_ANG_RATE_X = 14 # x-axis gyroscope angular rate +uint32 DATA_TYPE_GYRO_ANG_RATE_X = 14 # x-axis gyroscope angular rate # [deg/s *2^-12 signed] -uint32 DATA_TYPE_ACCELEROMETER_X = 16 # x-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_X = 16 # x-axis accelerometer specific # [force m/s^2 *2^-10 signed] -uint32 DATA_TYPE_ACCELEROMETER_Y = 17 # y-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_Y = 17 # y-axis accelerometer specific # [force m/s^2 *2^-10 signed] -uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific +uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific # [force m/s^2 *2^-10 signed] @@ -71,7 +71,7 @@ uint32 DATA_TYPE_ACCELEROMETER_Z = 18 # z-axis accelerometer specific # End of repeated block # Start of optional block (size is either 0 or 1) -uint32[] calibTtag # Receiver local time calibrated. +uint32[] calib_t_tag # Receiver local time calibrated. # This field must not be supplied when # calibTtagValid is set to 0 [ms] -# End of optional block \ No newline at end of file +# End of optional block diff --git a/ublox_msgs/msg/EsfRAW.msg b/ublox_msgs/msg/EsfRAW.msg index 52158d76..0f0a47f4 100644 --- a/ublox_msgs/msg/EsfRAW.msg +++ b/ublox_msgs/msg/EsfRAW.msg @@ -5,7 +5,7 @@ # to the GNSS chip. Possible data types for the data field are accelerometer, # gyroscope and temperature readings as described in the ESF Measurement Data # section. Note that the rate selected in CFG-MSG is not respected. If a -# positive rate is selected then all raw measurements will be output. +# positive rate is selected then all raw measurements will be output. # # Supported on ADR/UDR products. # @@ -15,4 +15,4 @@ uint8 MESSAGE_ID = 3 uint8[4] reserved0 # Reserved -EsfRAW_Block[] blocks \ No newline at end of file +EsfRAWBlock[] blocks diff --git a/ublox_msgs/msg/EsfRAW_Block.msg b/ublox_msgs/msg/EsfRAWBlock.msg similarity index 72% rename from ublox_msgs/msg/EsfRAW_Block.msg rename to ublox_msgs/msg/EsfRAWBlock.msg index c8375729..647dd3c2 100644 --- a/ublox_msgs/msg/EsfRAW_Block.msg +++ b/ublox_msgs/msg/EsfRAWBlock.msg @@ -3,6 +3,6 @@ uint32 data # Its scaling and unit depends on the type and is # the same as in ESF-MEAS uint32 DATA_FIELD_MASK = 16777215 -uint32 DATA_TYPE_MASK = 4278190080 # type of data +uint32 DATA_TYPE_MASK = 4278190080 # type of data # (0 = no data; 1..255 = data type) -uint32 sTtag # sensor time tag \ No newline at end of file +uint32 s_t_tag # sensor time tag diff --git a/ublox_msgs/msg/EsfSTATUS.msg b/ublox_msgs/msg/EsfSTATUS.msg index 18716da7..b3eb0d1d 100644 --- a/ublox_msgs/msg/EsfSTATUS.msg +++ b/ublox_msgs/msg/EsfSTATUS.msg @@ -7,28 +7,28 @@ uint8 CLASS_ID = 16 uint8 MESSAGE_ID = 16 -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint8 version # Message version (2 for this version) uint8[7] reserved1 # Reserved -uint8 fusionMode # Fusion mode: +uint8 fusion_mode # Fusion mode: uint8 FUSION_MODE_INIT = 0 # receiver is initializing some unknown values # required for doing sensor fusion -uint8 FUSION_MODE_FUSION = 1 # GNSS and sensor data are +uint8 FUSION_MODE_FUSION = 1 # GNSS and sensor data are # used for navigation solution computation -uint8 FUSION_MODE_SUSPENDED = 2 # sensor fusion is temporarily disabled - # due to e.g. invalid sensor data or detected +uint8 FUSION_MODE_SUSPENDED = 2 # sensor fusion is temporarily disabled + # due to e.g. invalid sensor data or detected # ferry -uint8 FUSION_MODE_DISABLED = 3 # sensor fusion is permanently disabled - # until receiver reset due e.g. to sensor +uint8 FUSION_MODE_DISABLED = 3 # sensor fusion is permanently disabled + # until receiver reset due e.g. to sensor # error uint8[2] reserved2 # Reserved -uint8 numSens # Number of sensors +uint8 num_sens # Number of sensors # Start of repeated block (numSens times) -EsfSTATUS_Sens[] sens -# End of repeated block \ No newline at end of file +EsfSTATUSSens[] sens +# End of repeated block diff --git a/ublox_msgs/msg/EsfSTATUSSens.msg b/ublox_msgs/msg/EsfSTATUSSens.msg new file mode 100644 index 00000000..59d39641 --- /dev/null +++ b/ublox_msgs/msg/EsfSTATUSSens.msg @@ -0,0 +1,7 @@ +# See Esf-STATUS +# + +uint8 sens_status1 # Sensor status, part 1 (see graphic below) +uint8 sens_status2 # Sensor status, part 2 (see graphic below) +uint8 freq # Observation frequency [Hz] +uint8 faults # Sensor faults (see graphic below) diff --git a/ublox_msgs/msg/HnrPVT.msg b/ublox_msgs/msg/HnrPVT.msg index 5a926f9a..632b74ed 100644 --- a/ublox_msgs/msg/HnrPVT.msg +++ b/ublox_msgs/msg/HnrPVT.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message provides the position, velocity and time solution with high +# This message provides the position, velocity and time solution with high # output rate. # # Supported on ADR and UDR products. @@ -12,7 +12,7 @@ uint8 CLASS_ID = 40 uint8 MESSAGE_ID = 0 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -22,21 +22,21 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 gpsFix # GPS fix Type, range 0..5 +uint8 gps_fix # GPS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GPS_DEAD_RECKONING_COMBINED = 4 # GPS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix uint8 flags # Fix Status Flags uint8 FLAGS_GNSS_FIX_OK = 1 # i.e. within DOP & accuracy masks @@ -50,18 +50,18 @@ uint8[2] reserved0 # Reserved int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] +int32 h_msl # Height above mean sea level [mm] -int32 gSpeed # Ground Speed (2-D) [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] int32 speed # Speed (3-D) [mm/s] -int32 headMot # Heading of motion (2-D) [deg / 1e-5] -int32 headVeh # Heading of vehicle (2-D) [deg / 1e-5] +int32 head_mot # Heading of motion (2-D) [deg / 1e-5] +int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) # [deg / 1e-5] -uint8[4] reserved1 # Reserved \ No newline at end of file +uint8[4] reserved1 # Reserved diff --git a/ublox_msgs/msg/MgaGAL.msg b/ublox_msgs/msg/MgaGAL.msg index 55a6c985..76140930 100644 --- a/ublox_msgs/msg/MgaGAL.msg +++ b/ublox_msgs/msg/MgaGAL.msg @@ -1,9 +1,9 @@ # MGA-GAL (0x13 0x02) # Galileo Ephemeris Assistance # -# This message allows the delivery of Galileo ephemeris assistance to a +# This message allows the delivery of Galileo ephemeris assistance to a # receiver. See the description of AssistNow Online for details. -# +# uint8 CLASS_ID = 19 uint8 MESSAGE_ID = 2 @@ -14,48 +14,48 @@ uint8 svid # Galileo Satellite identifier uint8 reserved0 # Reserved -uint16 iodNav # Ephemeris and clock correction issue of Data -int16 deltaN # Mean motion difference from computed value +uint16 iod_nav # Ephemeris and clock correction issue of Data +int16 delta_n # Mean motion difference from computed value # [semi-cir cles/s * 2^-43] int32 m0 # Mean anomaly at reference time [semi-cir cles 2^-31] uint32 e # Eccentricity [2^-33] -uint32 sqrtA # Square root of the semi-major axis [m^0.5 * 2^-19] +uint32 sqrt_a # Square root of the semi-major axis [m^0.5 * 2^-19] int32 omega0 # Longitude of ascending node of orbital plane at weekly # epoch [semi-cir cles 2^-31] -int32 i0 # inclination angle at reference time +int32 i0 # inclination angle at reference time # [semi-cir cles 2^-31] int32 omega # Argument of perigee [semi-cir cles 2^-31] -int32 omegaDot # Rate of change of right ascension +int32 omega_dot # Rate of change of right ascension # [semi-cir cles/s 2^-43] -int16 iDot # Rate of change of inclination angle +int16 i_dot # Rate of change of inclination angle # [semi-cir cles/s 2^-43] -int16 cuc # Amplitude of the cosine harmonic correction term to +int16 cuc # Amplitude of the cosine harmonic correction term to # the argument of latitude [radians * 2^-29] -int16 cus # Amplitude of the sine harmonic correction term to +int16 cus # Amplitude of the sine harmonic correction term to # the argument of latitude [radians * 2^-29] -int16 crc # Amplitude of the cosine harmonic correction term +int16 crc # Amplitude of the cosine harmonic correction term # to the orbit radius [radians * 2^-5] -int16 crs # Amplitude of the sine harmonic correction term to the +int16 crs # Amplitude of the sine harmonic correction term to the # orbit radius [radians * 2^-5] -int16 cic # Amplitude of the cosine harmonic correction term to +int16 cic # Amplitude of the cosine harmonic correction term to # the angle of inclination [radians * 2^-29] -int16 cis # Amplitude of the sine harmonic correction term to the +int16 cis # Amplitude of the sine harmonic correction term to the # angle of inclination [radians * 2^-29] uint16 toe # Ephemeris reference time [60 * s] int32 af0 # clock bias correction coefficient [s * 2^-34] int32 af1 # SV clock drift correction coefficient [s/s * 2^-46] -int8 af2 # SV clock drift rate correction coefficient +int8 af2 # SV clock drift rate correction coefficient # [s/s^2 * 2^-59] -uint8 sisaindexE1E5b # Signal-in-Space Accuracy index for dual frequency +uint8 sisaindex_e1_e5b # Signal-in-Space Accuracy index for dual frequency # E1-E5b uint16 toc # Clock correction data reference Time of Week [60 * s] -int16 bgdE1E5b # E1-E5b Broadcast Group Delay +int16 bgd_e1_e5b # E1-E5b Broadcast Group Delay uint8[2] reserved1 # Reserved -uint8 healthE1B # E1-B Signal Health Status -uint8 dataValidityE1B # E1-B Data Validity Status -uint8 healthE5b # E5b Signal Health Status -uint8 dataValidityE5b # E5b Data Validity Status +uint8 health_e1b # E1-B Signal Health Status +uint8 data_validity_e1b # E1-B Data Validity Status +uint8 health_e5b # E5b Signal Health Status +uint8 data_validity_e5b # E5b Data Validity Status -uint8[4] reserved2 # Reserved \ No newline at end of file +uint8[4] reserved2 # Reserved diff --git a/ublox_msgs/msg/MonGNSS.msg b/ublox_msgs/msg/MonGNSS.msg index 431a48dc..9d379f2b 100644 --- a/ublox_msgs/msg/MonGNSS.msg +++ b/ublox_msgs/msg/MonGNSS.msg @@ -3,7 +3,7 @@ # # This message reports major GNSS selection. Augmentation systems are not # reported. -# +# uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 40 @@ -16,16 +16,16 @@ uint8 BIT_MASK_BEIDOU = 4 uint8 BIT_MASK_GALILEO = 8 uint8 supported # The major GNSS that can be supported by this receiver -uint8 defaultGnss # Default major GNSS selection. If the default major GNSS - # selection is currently configured in the efuse for this - # receiver, it takes precedence over the default major - # GNSS selection configured in the executing firmware of +uint8 default_gnss # Default major GNSS selection. If the default major GNSS + # selection is currently configured in the efuse for this + # receiver, it takes precedence over the default major + # GNSS selection configured in the executing firmware of # this receiver. # see bit mask constants uint8 enabled # Current major GNSS selection enabled for this receiver # see bit mask constants -uint8 simultaneous # Maximum number of concurrent major GNSS that can be +uint8 simultaneous # Maximum number of concurrent major GNSS that can be # supported by this receiver uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/msg/MonHW.msg b/ublox_msgs/msg/MonHW.msg index 3203269e..e4e5291b 100644 --- a/ublox_msgs/msg/MonHW.msg +++ b/ublox_msgs/msg/MonHW.msg @@ -1,7 +1,7 @@ # MON-HW (0x0A 0x09) # Hardware Status # -# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral +# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral # Pins, Noise Level, Automatic Gain Control (AGC) # # WARNING: this message is a different length than the MonHW message for @@ -10,29 +10,29 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 9 -uint32 pinSel # Mask of Pins Set as Peripheral/PIO -uint32 pinBank # Mask of Pins Set as Bank A/B -uint32 pinDir # Mask of Pins Set as Input/Output -uint32 pinVal # Mask of Pins Value Low/High -uint16 noisePerMS # Noise Level as measured by the GPS Core -uint16 agcCnt # AGC Monitor (counts SIGHI xor SIGLO, +uint32 pin_sel # Mask of Pins Set as Peripheral/PIO +uint32 pin_bank # Mask of Pins Set as Bank A/B +uint32 pin_dir # Mask of Pins Set as Input/Output +uint32 pin_val # Mask of Pins Value Low/High +uint16 noise_per_ms # Noise Level as measured by the GPS Core +uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, # range 0 to 8191) -uint8 aStatus # Status of the Antenna Supervisor State Machine +uint8 a_status # Status of the Antenna Supervisor State Machine uint8 A_STATUS_INIT = 0 uint8 A_STATUS_UNKNOWN = 1 uint8 A_STATUS_OK = 2 uint8 A_STATUS_SHORT = 3 uint8 A_STATUS_OPEN = 4 -uint8 aPower # Current PowerStatus of Antenna -uint8 A_POWER_OFF = 0 +uint8 a_power # Current PowerStatus of Antenna +uint8 A_POWER_OFF = 0 uint8 A_POWER_ON = 1 uint8 A_POWER_UNKNOWN = 2 uint8 flags # Flags: uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) -uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: +uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK @@ -40,18 +40,18 @@ uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent # (not supported in protocol versions < 18) uint8 reserved0 # Reserved -uint32 usedMask # Mask of Pins that are used by the Virtual Pin +uint32 used_mask # Mask of Pins that are used by the Virtual Pin # Manager -uint8[17] VP # Array of Pin Mappings for each of the 17 +uint8[17] vp # Array of Pin Mappings for each of the 17 # Physical Pins -uint8 jamInd # CW Jamming indicator, scaled: +uint8 jam_ind # CW Jamming indicator, scaled: uint8 JAM_IND_NONE = 0 # No CW Jamming -uint8 JAM_IND_STRONG = 255 # Strong CW Jamming +uint8 JAM_IND_STRONG = 255 # Strong CW Jamming uint8[2] reserved1 # Reserved -uint32 pinIrq # Mask of Pins Value using the PIO Irq -uint32 pullH # Mask of Pins Value using the PIO Pull High +uint32 pin_irq # Mask of Pins Value using the PIO Irq +uint32 pull_h # Mask of Pins Value using the PIO Pull High # Resistor -uint32 pullL # Mask of Pins Value using the PIO Pull Low +uint32 pull_l # Mask of Pins Value using the PIO Pull Low # Resistor diff --git a/ublox_msgs/msg/MonHW6.msg b/ublox_msgs/msg/MonHW6.msg index b6e1ba91..6de85912 100644 --- a/ublox_msgs/msg/MonHW6.msg +++ b/ublox_msgs/msg/MonHW6.msg @@ -2,7 +2,7 @@ # Hardware Status # Firmware 6 # -# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral +# Status of different aspect of the hardware, such as Antenna, PIO/Peripheral # Pins, Noise Level, Automatic Gain Control (AGC) # # WARNING: this message is a different length than the MonHW message for @@ -11,29 +11,29 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 9 -uint32 pinSel # Mask of Pins Set as Peripheral/PIO -uint32 pinBank # Mask of Pins Set as Bank A/B -uint32 pinDir # Mask of Pins Set as Input/Output -uint32 pinVal # Mask of Pins Value Low/High -uint16 noisePerMS # Noise Level as measured by the GPS Core -uint16 agcCnt # AGC Monitor (counts SIGHI xor SIGLO, +uint32 pin_sel # Mask of Pins Set as Peripheral/PIO +uint32 pin_bank # Mask of Pins Set as Bank A/B +uint32 pin_dir # Mask of Pins Set as Input/Output +uint32 pin_val # Mask of Pins Value Low/High +uint16 noise_per_ms # Noise Level as measured by the GPS Core +uint16 agc_cnt # AGC Monitor (counts SIGHI xor SIGLO, # range 0 to 8191) -uint8 aStatus # Status of the Antenna Supervisor State Machine +uint8 a_status # Status of the Antenna Supervisor State Machine uint8 A_STATUS_INIT = 0 uint8 A_STATUS_UNKNOWN = 1 uint8 A_STATUS_OK = 2 uint8 A_STATUS_SHORT = 3 uint8 A_STATUS_OPEN = 4 -uint8 aPower # Current PowerStatus of Antenna -uint8 A_POWER_OFF = 0 +uint8 a_power # Current PowerStatus of Antenna +uint8 A_POWER_OFF = 0 uint8 A_POWER_ON = 1 uint8 A_POWER_UNKNOWN = 2 uint8 flags # Flags: uint8 FLAGS_RTC_CALIB = 1 # RTC is calibrated uint8 FLAGS_SAFE_BOOT = 2 # Safe boot mode (0 = inactive, 1 = active) -uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: +uint8 FLAGS_JAMMING_STATE_MASK = 12 # output from Jamming/Interference Monitor: uint8 JAMMING_STATE_UNKNOWN_OR_DISABLED = 0 # unknown or feature disabled uint8 JAMMING_STATE_OK = 4 # ok - no significant jamming uint8 JAMMING_STATE_WARNING = 8 # interference visible but fix OK @@ -41,18 +41,18 @@ uint8 JAMMING_STATE_CRITICAL = 12 # interference visible and no fix uint8 FLAGS_XTAL_ABSENT = 16 # RTC XTAL is absent # (not supported in protocol versions < 18) uint8 reserved0 # Reserved -uint32 usedMask # Mask of Pins that are used by the Virtual Pin +uint32 used_mask # Mask of Pins that are used by the Virtual Pin # Manager -uint8[25] VP # Array of Pin Mappings for each of the 25 +uint8[25] vp # Array of Pin Mappings for each of the 25 # Physical Pins -uint8 jamInd # CW Jamming indicator, scaled: +uint8 jam_ind # CW Jamming indicator, scaled: uint8 JAM_IND_NONE = 0 # No CW Jamming -uint8 JAM_IND_STRONG = 255 # Strong CW Jamming +uint8 JAM_IND_STRONG = 255 # Strong CW Jamming uint8[2] reserved1 # Reserved -uint32 pinIrq # Mask of Pins Value using the PIO Irq -uint32 pullH # Mask of Pins Value using the PIO Pull High +uint32 pin_irq # Mask of Pins Value using the PIO Irq +uint32 pull_h # Mask of Pins Value using the PIO Pull High + # Resistor +uint32 pull_l # Mask of Pins Value using the PIO Pull Low # Resistor -uint32 pullL # Mask of Pins Value using the PIO Pull Low - # Resistor \ No newline at end of file diff --git a/ublox_msgs/msg/MonVER.msg b/ublox_msgs/msg/MonVER.msg index 87fbb912..2ce3c6d3 100644 --- a/ublox_msgs/msg/MonVER.msg +++ b/ublox_msgs/msg/MonVER.msg @@ -6,9 +6,9 @@ uint8 CLASS_ID = 10 uint8 MESSAGE_ID = 4 -char[30] swVersion # Zero-terminated software version string. -char[10] hwVersion # Zero-terminated hardware version string. +char[30] sw_version # Zero-terminated software version string. +char[10] hw_version # Zero-terminated hardware version string. # Start of repeated block (N times) -MonVER_Extension[] extension +MonVERExtension[] extension # End of repeated block diff --git a/ublox_msgs/msg/MonVER_Extension.msg b/ublox_msgs/msg/MonVERExtension.msg similarity index 100% rename from ublox_msgs/msg/MonVER_Extension.msg rename to ublox_msgs/msg/MonVERExtension.msg diff --git a/ublox_msgs/msg/NavATT.msg b/ublox_msgs/msg/NavATT.msg index 61371fd9..d6f2774b 100644 --- a/ublox_msgs/msg/NavATT.msg +++ b/ublox_msgs/msg/NavATT.msg @@ -8,7 +8,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 5 -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint8 version # Message version (0 for this version) uint8[3] reserved0 # Reserved @@ -16,8 +16,8 @@ uint8[3] reserved0 # Reserved int32 roll # Vehicle roll. [deg / 1e-5] int32 pitch # Vehicle pitch. [deg / 1e-5] int32 heading # Vehicle heading. [deg / 1e-5] -uint32 accRoll # Vehicle roll accuracy (if null, roll angle is not +uint32 acc_roll # Vehicle roll accuracy (if null, roll angle is not # available). [deg / 1e-5] -uint32 accPitch # Vehicle pitch accuracy (if null, pitch angle is not +uint32 acc_pitch # Vehicle pitch accuracy (if null, pitch angle is not # available). [deg / 1e-5] -uint32 accHeading # Vehicle heading accuracy [deg / 1e-5] \ No newline at end of file +uint32 acc_heading # Vehicle heading accuracy [deg / 1e-5] diff --git a/ublox_msgs/msg/NavCLOCK.msg b/ublox_msgs/msg/NavCLOCK.msg index 7edfd188..1d8b6472 100644 --- a/ublox_msgs/msg/NavCLOCK.msg +++ b/ublox_msgs/msg/NavCLOCK.msg @@ -5,9 +5,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 34 -uint32 iTOW # GPS Millisecond Time of week [ms] +uint32 i_tow # GPS Millisecond Time of week [ms] -int32 clkB # Clock bias in nanoseconds [ns] -int32 clkD # Clock drift in nanoseconds per second [ns/s] -uint32 tAcc # Time Accuracy Estimate [ns] -uint32 fAcc # Frequency Accuracy Estimate [ps/s] +int32 clk_b # Clock bias in nanoseconds [ns] +int32 clk_d # Clock drift in nanoseconds per second [ns/s] +uint32 t_acc # Time Accuracy Estimate [ns] +uint32 f_acc # Frequency Accuracy Estimate [ps/s] diff --git a/ublox_msgs/msg/NavDGPS.msg b/ublox_msgs/msg/NavDGPS.msg index 04b8f296..47360edc 100644 --- a/ublox_msgs/msg/NavDGPS.msg +++ b/ublox_msgs/msg/NavDGPS.msg @@ -8,12 +8,12 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 49 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] int32 age # Age of newest correction data [ms] -int16 baseId # DGPS Base Station ID -int16 baseHealth # DGPS Base Station Health Status -int8 numCh # Number of channels for which correction data is +int16 base_id # DGPS Base Station ID +int16 base_health # DGPS Base Station Health Status +int8 num_ch # Number of channels for which correction data is # following uint8 status # DGPS Correction Type Status @@ -22,4 +22,4 @@ uint8 DGPS_CORRECTION_PR_PRR = 1 uint16 reserved1 # Reserved -NavDGPS_SV[] sv +NavDGPSSV[] sv diff --git a/ublox_msgs/msg/NavDGPS_SV.msg b/ublox_msgs/msg/NavDGPSSV.msg similarity index 87% rename from ublox_msgs/msg/NavDGPS_SV.msg rename to ublox_msgs/msg/NavDGPSSV.msg index a501c0a8..5dc00d46 100644 --- a/ublox_msgs/msg/NavDGPS_SV.msg +++ b/ublox_msgs/msg/NavDGPSSV.msg @@ -7,7 +7,7 @@ uint8 FLAGS_CHANNEL_MASK = 15 # Bitmask for channel number, range 0..15 # Channel numbers > 15 marked as 15 uint8 FLAGS_DGPS = 16 # DGPS Used for this SV -uint16 ageC # Age of latest correction data [ms] +uint16 age_c # Age of latest correction data [ms] float32 prc # Pseudo Range Correction [m] float32 prrc # Pseudo Range Rate Correction [m/s] diff --git a/ublox_msgs/msg/NavDOP.msg b/ublox_msgs/msg/NavDOP.msg index cb350c0c..ed837c2e 100644 --- a/ublox_msgs/msg/NavDOP.msg +++ b/ublox_msgs/msg/NavDOP.msg @@ -2,19 +2,19 @@ # Dilution of precision # # - DOP values are dimensionless. -# - All DOP values are scaled by a factor of 100. If the unit transmits a value +# - All DOP values are scaled by a factor of 100. If the unit transmits a value # of e.g. 156, the DOP value is 1.56. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 4 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] -uint16 gDOP # Geometric DOP [1 / 0.01] -uint16 pDOP # Position DOP [1 / 0.01] -uint16 tDOP # Time DOP [1 / 0.01] -uint16 vDOP # Vertical DOP [1 / 0.01] -uint16 hDOP # Horizontal DOP [1 / 0.01] -uint16 nDOP # Northing DOP [1 / 0.01] -uint16 eDOP # Easting DOP [1 / 0.01] +uint16 g_dop # Geometric DOP [1 / 0.01] +uint16 p_dop # Position DOP [1 / 0.01] +uint16 t_dop # Time DOP [1 / 0.01] +uint16 v_dop # Vertical DOP [1 / 0.01] +uint16 h_dop # Horizontal DOP [1 / 0.01] +uint16 n_dop # Northing DOP [1 / 0.01] +uint16 e_dop # Easting DOP [1 / 0.01] diff --git a/ublox_msgs/msg/NavPOSECEF.msg b/ublox_msgs/msg/NavPOSECEF.msg index f5aba86a..b1f4560c 100644 --- a/ublox_msgs/msg/NavPOSECEF.msg +++ b/ublox_msgs/msg/NavPOSECEF.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 1 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] -int32 ecefX # ECEF X coordinate [cm] -int32 ecefY # ECEF Y coordinate [cm] -int32 ecefZ # ECEF Z coordinate [cm] -uint32 pAcc # Position Accuracy Estimate [cm] +int32 ecef_x # ECEF X coordinate [cm] +int32 ecef_y # ECEF Y coordinate [cm] +int32 ecef_z # ECEF Z coordinate [cm] +uint32 p_acc # Position Accuracy Estimate [cm] diff --git a/ublox_msgs/msg/NavPOSLLH.msg b/ublox_msgs/msg/NavPOSLLH.msg index 8a3f2552..6f6e5464 100644 --- a/ublox_msgs/msg/NavPOSLLH.msg +++ b/ublox_msgs/msg/NavPOSLLH.msg @@ -3,7 +3,7 @@ # # See important comments concerning validity of position given in section # Navigation Output Filters. -# This message outputs the Geodetic position in the currently selected +# This message outputs the Geodetic position in the currently selected # Ellipsoid. The default is the WGS84 Ellipsoid, but can be changed with the # message CFG-DAT. # @@ -11,11 +11,11 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 2 -uint32 iTOW # GPS Millisecond Time of Week [ms] +uint32 i_tow # GPS Millisecond Time of Week [ms] int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] diff --git a/ublox_msgs/msg/NavPVT.msg b/ublox_msgs/msg/NavPVT.msg index d60778ef..847b3bce 100644 --- a/ublox_msgs/msg/NavPVT.msg +++ b/ublox_msgs/msg/NavPVT.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message combines Position, velocity and time solution in LLH, +# This message combines Position, velocity and time solution in LLH, # including accuracy figures # # WARNING: For firmware version 7, this message is a different length. @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -23,22 +23,22 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 tAcc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 fixType # GNSS fix Type, range 0..5 +uint8 fix_type # GNSS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED = 4 # GNSS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision # devices) uint8 flags # Fix Status Flags @@ -52,41 +52,41 @@ uint8 PSM_TRACKING = 12 # Tracking uint8 PSM_POWER_OPTIMIZED_TRACKING = 16 # Power Optimized Tracking uint8 PSM_INACTIVE = 20 # Inactive uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid -uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status +uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status uint8 CARRIER_PHASE_NO_SOLUTION = 0 # no carrier phase range solution -uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed - # integer measurements have been used to +uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed + # integer measurements have been used to # calculate the solution) -uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer - # carrier phase range measurements have +uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer + # carrier phase range measurements have # been used to calculate the solution) uint8 flags2 # Additional Flags -uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of +uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of # Day validity confirmation is available uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed -uint8 numSV # Number of SVs used in Nav Solution +uint8 num_sv # Number of SVs used in Nav Solution int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] -int32 velN # NED north velocity [mm/s] -int32 velE # NED east velocity [mm/s] -int32 velD # NED down velocity [mm/s] -int32 gSpeed # Ground Speed (2-D) [mm/s] +int32 vel_n # NED north velocity [mm/s] +int32 vel_e # NED east velocity [mm/s] +int32 vel_d # NED down velocity [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) # [deg / 1e-5] -uint16 pDOP # Position DOP [1 / 0.01] +uint16 p_dop # Position DOP [1 / 0.01] uint8[6] reserved1 # Reserved -int32 headVeh # Heading of vehicle (2-D) [deg / 1e-5] -int16 magDec # Magnetic declination [deg / 1e-2] -uint16 magAcc # Magnetic declination accuracy [deg / 1e-2] +int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] +int16 mag_dec # Magnetic declination [deg / 1e-2] +uint16 mag_acc # Magnetic declination accuracy [deg / 1e-2] diff --git a/ublox_msgs/msg/NavPVT7.msg b/ublox_msgs/msg/NavPVT7.msg index b3b37957..6ec136e6 100644 --- a/ublox_msgs/msg/NavPVT7.msg +++ b/ublox_msgs/msg/NavPVT7.msg @@ -4,7 +4,7 @@ # Note that during a leap second there may be more (or less) than 60 seconds in # a minute; see the description of leap seconds for details. # -# This message combines Position, velocity and time solution in LLH, +# This message combines Position, velocity and time solution in LLH, # including accuracy figures # # WARNING: For firmware version 7, this message is a different length. @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -23,22 +23,22 @@ uint8 sec # Seconds of minute, range 0..60 (UTC) uint8 valid # Validity flags uint8 VALID_DATE = 1 # Valid UTC Date -uint8 VALID_TIME = 2 # Valid -uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved +uint8 VALID_TIME = 2 # Valid +uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 tAcc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) -uint8 fixType # GNSS fix Type, range 0..5 +uint8 fix_type # GNSS fix Type, range 0..5 uint8 FIX_TYPE_NO_FIX = 0 uint8 FIX_TYPE_DEAD_RECKONING_ONLY = 1 -uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, +uint8 FIX_TYPE_2D = 2 # Signal from only 3 SVs, # constant altitude assumed uint8 FIX_TYPE_3D = 3 uint8 FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED = 4 # GNSS + Dead reckoning -uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision +uint8 FIX_TYPE_TIME_ONLY = 5 # Time only fix (High precision # devices) uint8 flags # Fix Status Flags @@ -52,37 +52,37 @@ uint8 PSM_TRACKING = 12 # Tracking uint8 PSM_POWER_OPTIMIZED_TRACKING = 16 # Power Optimized Tracking uint8 PSM_INACTIVE = 20 # Inactive uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid -uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status +uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status uint8 CARRIER_PHASE_NO_SOLUTION = 0 # no carrier phase range solution -uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed - # integer measurements have been used to +uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed + # integer measurements have been used to # calculate the solution) -uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer - # carrier phase range measurements have +uint8 CARRIER_PHASE_FIXED = 128 # fixed solution (>=1 fixed integer + # carrier phase range measurements have # been used to calculate the solution) uint8 flags2 # Additional Flags -uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of +uint8 FLAGS2_CONFIRMED_AVAILABLE = 32 # information about UTC Date and Time of # Day validity confirmation is available uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed -uint8 numSV # Number of SVs used in Nav Solution +uint8 num_sv # Number of SVs used in Nav Solution int32 lon # Longitude [deg / 1e-7] int32 lat # Latitude [deg / 1e-7] int32 height # Height above Ellipsoid [mm] -int32 hMSL # Height above mean sea level [mm] -uint32 hAcc # Horizontal Accuracy Estimate [mm] -uint32 vAcc # Vertical Accuracy Estimate [mm] +int32 h_msl # Height above mean sea level [mm] +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] -int32 velN # NED north velocity [mm/s] -int32 velE # NED east velocity [mm/s] -int32 velD # NED down velocity [mm/s] -int32 gSpeed # Ground Speed (2-D) [mm/s] +int32 vel_n # NED north velocity [mm/s] +int32 vel_e # NED east velocity [mm/s] +int32 vel_d # NED down velocity [mm/s] +int32 g_speed # Ground Speed (2-D) [mm/s] int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [mm/s] -uint32 headAcc # Heading Accuracy Estimate (both motion & vehicle) +uint32 s_acc # Speed Accuracy Estimate [mm/s] +uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) # [deg / 1e-5] -uint16 pDOP # Position DOP [1 / 0.01] -uint8[6] reserved1 # Reserved \ No newline at end of file +uint16 p_dop # Position DOP [1 / 0.01] +uint8[6] reserved1 # Reserved diff --git a/ublox_msgs/msg/NavRELPOSNED.msg b/ublox_msgs/msg/NavRELPOSNED.msg index e6bd26b3..14b574ba 100644 --- a/ublox_msgs/msg/NavRELPOSNED.msg +++ b/ublox_msgs/msg/NavRELPOSNED.msg @@ -1,6 +1,6 @@ # NAV-RELPOSNED (0x01 0x3C) # Relative Positioning Information in NED frame -# +# # The NED frame is defined as the local topological system at the reference # station. The relative position vector components in this message, along with # their associated accuracies, are given in that local topological system @@ -18,31 +18,31 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved0 # Reserved -uint16 refStationId # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 iTow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 relPosN # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 relPosE # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 relPosD # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int8 relPosHPN # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 relPosHPE # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. - # The full East component of the relative + # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 relPosHPD # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative @@ -51,35 +51,35 @@ int8 relPosHPD # High-precision Down component of relative uint8 reserved1 # Reserved -uint32 accN # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 accE # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 accD # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] uint32 flags -uint32 FLAGS_GNSS_FIX_OK = 1 # A valid fix (i.e within DOP & accuracy +uint32 FLAGS_GNSS_FIX_OK = 1 # A valid fix (i.e within DOP & accuracy # masks) uint32 FLAGS_DIFF_SOLN = 2 # Set if differential corrections were applied uint32 FLAGS_REL_POS_VALID = 4 # Set if relative position components and # accuracies are valid uint32 FLAGS_CARR_SOLN_MASK = 24 # Carrier phase range solution status: uint32 FLAGS_CARR_SOLN_NONE = 0 # No carrier phase range solution -uint32 FLAGS_CARR_SOLN_FLOAT = 8 # Float solution. No fixed integer carrier +uint32 FLAGS_CARR_SOLN_FLOAT = 8 # Float solution. No fixed integer carrier # phase measurements have been used to # calculate the solution uint32 FLAGS_CARR_SOLN_FIXED = 16 # Fixed solution. One or more fixed # integer carrier phase range measurements # have been used to calculate the solution -uint32 FLAGS_IS_MOVING = 32 # if the receiver is operating in moving +uint32 FLAGS_IS_MOVING = 32 # if the receiver is operating in moving # baseline mode (not supported in protocol # versions less than 20.3) uint32 FLAGS_REF_POS_MISS = 64 # Set if extrapolated reference position was - # used to compute moving baseline solution + # used to compute moving baseline solution # this epoch (not supported in protocol - # versions less than 20.3) + # versions less than 20.3) uint32 FLAGS_REF_OBS_MISS = 128 # Set if extrapolated reference observations # were used to compute moving baseline # solution this epoch (not supported in - # protocol versions less than 20.3) \ No newline at end of file + # protocol versions less than 20.3) diff --git a/ublox_msgs/msg/NavRELPOSNED9.msg b/ublox_msgs/msg/NavRELPOSNED9.msg index 679cca2f..13f3e89d 100644 --- a/ublox_msgs/msg/NavRELPOSNED9.msg +++ b/ublox_msgs/msg/NavRELPOSNED9.msg @@ -17,56 +17,56 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved1 # Reserved -uint16 refStationId # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 iTow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 relPosN # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 relPosE # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 relPosD # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int32 relPosLength # Length of the relative position vector +int32 rel_pos_length # Length of the relative position vector # [cm] -int32 relPosHeading # Heading of the relative position vector +int32 rel_pos_heading # Heading of the relative position vector # [1e-5 deg] uint8[4] reserved2 # Reserved -int8 relPosHPN # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 relPosHPE # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 relPosHPD # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative # position vector, in units of cm, is given by # relPosD + (relPosHPD * 1e-2) -int8 relPosHPLength # High-precision component of the length of +int8 rel_pos_hp_length # High-precision component of the length of # the relative position vector. # Must be in the range -99 to +99. # The full length of the relative position # vector, in units of cm, is given by # relPosLength + (relPosHPLength * 1e-2) -uint32 accN # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 accE # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 accD # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] -uint32 accLength # Accuracy of length of the relative position +uint32 acc_length # Accuracy of length of the relative position # vector [0.1 mm] -uint32 accHeading # Accuracy of heading of the relative position +uint32 acc_heading # Accuracy of heading of the relative position # vector [0.1 mm] uint8[4] reserved3 # Reserved @@ -103,4 +103,4 @@ uint32 FLAGS_REL_POS_HEAD_VALID = 256 # Set if extrapolated reference observat uint32 FLAGS_REL_POS_NORM = 512 # Set if extrapolated reference observations # were used to compute moving baseline # solution this epoch (not supported in - # protocol versions less than 20.3) \ No newline at end of file + # protocol versions less than 20.3) diff --git a/ublox_msgs/msg/NavSAT.msg b/ublox_msgs/msg/NavSAT.msg index 348782d3..a31689e7 100644 --- a/ublox_msgs/msg/NavSAT.msg +++ b/ublox_msgs/msg/NavSAT.msg @@ -1,18 +1,18 @@ # NAV-SAT (0x01 0x35) # Satellite Information # -# This message displays information about SVs which are either known to be +# This message displays information about SVs which are either known to be # visible or currently tracked by the receiver. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 53 -uint32 iTOW # GPS time of week of the navigation epoch. [ms] +uint32 i_tow # GPS time of week of the navigation epoch. [ms] uint8 version # Message version (1 for this version) -uint8 numSvs # Number of satellites -uint8[2] reserved0 # Reserved +uint8 num_svs # Number of satellites +uint8[2] reserved0 # Reserved # start of repeated block (numSvs times) -NavSAT_SV[] sv -# end of repeated block \ No newline at end of file +NavSATSV[] sv +# end of repeated block diff --git a/ublox_msgs/msg/NavSAT_SV.msg b/ublox_msgs/msg/NavSATSV.msg similarity index 87% rename from ublox_msgs/msg/NavSAT_SV.msg rename to ublox_msgs/msg/NavSATSV.msg index 476dab37..1d97290a 100644 --- a/ublox_msgs/msg/NavSAT_SV.msg +++ b/ublox_msgs/msg/NavSATSV.msg @@ -1,80 +1,80 @@ # see NAV-SAT message # -uint8 gnssId # GNSS identifier -uint8 svId # Satellite identifier +uint8 gnss_id # GNSS identifier +uint8 sv_id # Satellite identifier uint8 cno # Carrier to noise ratio (signal strength) ]dBHz int8 elev # Elevation (range: +/-90), unknown if out of range [deg] -int16 azim # Azimuth (range 0-360), unknown if elevation is out of range +int16 azim # Azimuth (range 0-360), unknown if elevation is out of range # [deg] -int16 prRes # Pseudo range residual [0.1 m] +int16 pr_res # Pseudo range residual [0.1 m] uint32 flags # Bitmask uint32 FLAGS_QUALITY_IND_MASK = 7 # Signal quality indicator: uint8 QUALITY_IND_NO_SIGNAL = 0 # no signal uint8 QUALITY_IND_SEARCHING_SIGNAL = 1 # searching signal uint8 QUALITY_IND_SIGNAL_ACQUIRED = 2 # signal acquired -uint8 QUALITY_IND_SIGNAL_DETECTED_BUT_UNUSABLE = 3 # signal detected but +uint8 QUALITY_IND_SIGNAL_DETECTED_BUT_UNUSABLE = 3 # signal detected but # unusable -uint8 QUALITY_IND_CODE_LOCKED_AND_TIME_SYNC = 4 # code locked and time +uint8 QUALITY_IND_CODE_LOCKED_AND_TIME_SYNC = 4 # code locked and time # synchronized -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC1 = 5 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC1 = 5 # code and carrier + # locked and time + # synchronized, # quality = 1 -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC2 = 6 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC2 = 6 # code and carrier + # locked and time + # synchronized, # quality = 2 -uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC3 = 7 # code and carrier - # locked and time - # synchronized, +uint8 QUALITY_IND_CODE_AND_CARR_LOCK_AND_TIME_SYNC3 = 7 # code and carrier + # locked and time + # synchronized, # quality = 3 -# Note: Since IMES signals are not time synchronized, a channel tracking an IMES +# Note: Since IMES signals are not time synchronized, a channel tracking an IMES # signal can never reach a quality indicator value of higher than 3. -uint32 FLAGS_SV_USED = 8 # whether SV is currently being +uint32 FLAGS_SV_USED = 8 # whether SV is currently being # used for navigation uint32 FLAGS_HEALTH_MASK = 48 # SV health flag: uint32 HEALTH_UNKNOWN = 0 # unknown uint32 HEALTH_HEALTHY = 1 # healthy uint32 HEALTH_UNHEALTHY = 2 # unhealthy -uint32 FLAGS_DIFF_CORR = 64 # whether differential correction +uint32 FLAGS_DIFF_CORR = 64 # whether differential correction # data is available for this SV -uint32 FLAGS_SMOOTHED = 128 # whether carrier smoothed +uint32 FLAGS_SMOOTHED = 128 # whether carrier smoothed # pseudorange used uint32 FLAGS_ORBIT_SOURCE_MASK = 1792 # Orbit source: -uint32 ORBIT_SOURCE_UNAVAILABLE = 0 # no orbit information is +uint32 ORBIT_SOURCE_UNAVAILABLE = 0 # no orbit information is # available for this SV uint32 ORBIT_SOURCE_EPH = 256 # ephemeris is used uint32 ORBIT_SOURCE_ALM = 512 # almanac is used -uint32 ORBIT_SOURCE_ASSIST_OFFLINE = 768 # AssistNow Offline orbit is +uint32 ORBIT_SOURCE_ASSIST_OFFLINE = 768 # AssistNow Offline orbit is # used -uint32 ORBIT_SOURCE_ASSIST_AUTONOMOUS = 1024 # AssistNow Autonomous orbit is +uint32 ORBIT_SOURCE_ASSIST_AUTONOMOUS = 1024 # AssistNow Autonomous orbit is # used -uint32 ORBIT_SOURCE_OTHER1 = 1280 # other orbit information is +uint32 ORBIT_SOURCE_OTHER1 = 1280 # other orbit information is # used -uint32 ORBIT_SOURCE_OTHER2 = 1536 # other orbit information is +uint32 ORBIT_SOURCE_OTHER2 = 1536 # other orbit information is # used -uint32 ORBIT_SOURCE_OTHER3 = 1792 # other orbit information is +uint32 ORBIT_SOURCE_OTHER3 = 1792 # other orbit information is # used -uint32 FLAGS_EPH_AVAIL = 2048 # whether ephemeris is available +uint32 FLAGS_EPH_AVAIL = 2048 # whether ephemeris is available # for this SV -uint32 FLAGS_ALM_AVAIL = 4096 # whether almanac is available for +uint32 FLAGS_ALM_AVAIL = 4096 # whether almanac is available for # this SV -uint32 FLAGS_ANO_AVAIL = 8192 # whether AssistNow Offline data +uint32 FLAGS_ANO_AVAIL = 8192 # whether AssistNow Offline data # is available for this SV -uint32 FLAGS_AOP_AVAIL = 16384 # whether AssistNow Autonomous +uint32 FLAGS_AOP_AVAIL = 16384 # whether AssistNow Autonomous # data is available for this SV -uint32 FLAGS_SBAS_CORR_USED = 65536 # whether SBAS corrections have +uint32 FLAGS_SBAS_CORR_USED = 65536 # whether SBAS corrections have # been used for this SV -uint32 FLAGS_RTCM_CORR_USED = 131072 # whether RTCM corrections have +uint32 FLAGS_RTCM_CORR_USED = 131072 # whether RTCM corrections have # been used for this SV -uint32 FLAGS_PR_CORR_USED = 1048576 # whether Pseudorange corrections +uint32 FLAGS_PR_CORR_USED = 1048576 # whether Pseudorange corrections # have been used for this SV -uint32 FLAGS_CR_CORR_USED = 2097152 # whether Carrier range - # corrections have been used for +uint32 FLAGS_CR_CORR_USED = 2097152 # whether Carrier range + # corrections have been used for + # this SV +uint32 FLAGS_DO_CORR_USED = 4194304 # whether Range rate (Doppler) + # corrections have been used for # this SV -uint32 FLAGS_DO_CORR_USED = 4194304 # whether Range rate (Doppler) - # corrections have been used for - # this SV \ No newline at end of file diff --git a/ublox_msgs/msg/NavSBAS.msg b/ublox_msgs/msg/NavSBAS.msg index b41409d6..311c9b40 100644 --- a/ublox_msgs/msg/NavSBAS.msg +++ b/ublox_msgs/msg/NavSBAS.msg @@ -7,9 +7,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 50 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 geo # PRN Number of the GEO where correction and integrity +uint8 geo # PRN Number of the GEO where correction and integrity # data is used from uint8 mode # SBAS Mode @@ -34,4 +34,4 @@ uint8 SERVICE_TESTMODE = 8 uint8 cnt # Number of SV data following uint8[3] reserved0 # Reserved -NavSBAS_SV[] sv +NavSBASSV[] sv diff --git a/ublox_msgs/msg/NavSBAS_SV.msg b/ublox_msgs/msg/NavSBASSV.msg similarity index 72% rename from ublox_msgs/msg/NavSBAS_SV.msg rename to ublox_msgs/msg/NavSBASSV.msg index 1ae4066a..c3be214f 100644 --- a/ublox_msgs/msg/NavSBAS_SV.msg +++ b/ublox_msgs/msg/NavSBASSV.msg @@ -4,8 +4,8 @@ uint8 svid # SV Id uint8 flags # Flags for this SV uint8 udre # Monitoring status -uint8 svSys # System (WAAS/EGNOS/...), same as SYS -uint8 svService # Services available, same as SERVICE +uint8 sv_sys # System (WAAS/EGNOS/...), same as SYS +uint8 sv_service # Services available, same as SERVICE uint8 reserved1 # Reserved int16 prc # Pseudo Range correction in [cm] uint16 reserved2 # Reserved diff --git a/ublox_msgs/msg/NavSOL.msg b/ublox_msgs/msg/NavSOL.msg index 4181605c..e36d85de 100644 --- a/ublox_msgs/msg/NavSOL.msg +++ b/ublox_msgs/msg/NavSOL.msg @@ -1,21 +1,21 @@ # NAV-SOL (0x01 0x06) # Navigation Solution Information # -# This message combines Position, velocity and time solution in ECEF, including +# This message combines Position, velocity and time solution in ECEF, including # accuracy figures -# This message has only been retained for backwards compatibility; users are +# This message has only been retained for backwards compatibility; users are # recommended to use the UBX-NAV-PVT message in preference. # uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 6 -uint32 iTOW # GPS Millisecond time of week [ms] -int32 fTOW # Fractional Nanoseconds remainder of rounded +uint32 i_tow # GPS Millisecond time of week [ms] +int32 f_tow # Fractional Nanoseconds remainder of rounded # ms above, range -500000 .. 500000 [ns] int16 week # GPS week (GPS time) -uint8 gpsFix # GPSfix Type, range 0..5 +uint8 gps_fix # GPSfix Type, range 0..5 uint8 GPS_NO_FIX = 0 uint8 GPS_DEAD_RECKONING_ONLY = 1 uint8 GPS_2D_FIX = 2 @@ -29,15 +29,15 @@ uint8 FLAGS_DIFF_SOLN = 2 # DGPS used uint8 FLAGS_WKNSET = 4 # Week Number valid uint8 FLAGS_TOWSET = 8 # Time of Week valid -int32 ecefX # ECEF X coordinate [cm] -int32 ecefY # ECEF Y coordinate [cm] -int32 ecefZ # ECEF Z coordinate [cm] -uint32 pAcc # 3D Position Accuracy Estimate [cm] -int32 ecefVX # ECEF X velocity [cm/s] -int32 ecefVY # ECEF Y velocity [cm/s] -int32 ecefVZ # ECEF Z velocity [cm/s] -uint32 sAcc # Speed Accuracy Estimate [cm/s] -uint16 pDOP # Position DOP [1 / 0.01] +int32 ecef_x # ECEF X coordinate [cm] +int32 ecef_y # ECEF Y coordinate [cm] +int32 ecef_z # ECEF Z coordinate [cm] +uint32 p_acc # 3D Position Accuracy Estimate [cm] +int32 ecef_vx # ECEF X velocity [cm/s] +int32 ecef_vy # ECEF Y velocity [cm/s] +int32 ecef_vz # ECEF Z velocity [cm/s] +uint32 s_acc # Speed Accuracy Estimate [cm/s] +uint16 p_dop # Position DOP [1 / 0.01] uint8 reserved1 # Reserved -uint8 numSV # Number of SVs used in Nav Solution +uint8 num_sv # Number of SVs used in Nav Solution uint32 reserved2 # Reserved diff --git a/ublox_msgs/msg/NavSTATUS.msg b/ublox_msgs/msg/NavSTATUS.msg index 8738e8ee..d9d07eea 100644 --- a/ublox_msgs/msg/NavSTATUS.msg +++ b/ublox_msgs/msg/NavSTATUS.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 3 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 gpsFix # GPSfix Type, this value does not qualify a fix as +uint8 gps_fix # GPSfix Type, this value does not qualify a fix as # valid and within the limits. See note on flag gpsFixOk # below uint8 GPS_NO_FIX = 0 @@ -21,41 +21,41 @@ uint8 GPS_GPS_DEAD_RECKONING_COMBINED = 4 uint8 GPS_TIME_ONLY_FIX = 5 uint8 flags # Navigation Status Flags -uint8 FLAGS_GPS_FIX_OK = 1 # position & velocity valid & within DOP & ACC +uint8 FLAGS_GPS_FIX_OK = 1 # position & velocity valid & within DOP & ACC # Masks uint8 FLAGS_DIFF_SOLN = 2 # Differential corrections were applied uint8 FLAGS_WKNSET = 4 # Week Number valid uint8 FLAGS_TOWSET = 8 # Time of Week valid -uint8 fixStat # Fix Status Information +uint8 fix_stat # Fix Status Information uint8 FIX_STAT_DIFF_CORR_MASK = 1 # 1 = differential corrections available # map matching status: uint8 FIX_STAT_MAP_MATCHING_MASK = 192 uint8 MAP_MATCHING_NONE = 0 # none -uint8 MAP_MATCHING_VALID = 64 # valid but not used, i.e. map matching data +uint8 MAP_MATCHING_VALID = 64 # valid but not used, i.e. map matching data # was received, but was too old uint8 MAP_MATCHING_USED = 128 # valid and used, map matching data was applied -uint8 MAP_MATCHING_DR = 192 # valid and used, map matching data was +uint8 MAP_MATCHING_DR = 192 # valid and used, map matching data was # applied. In case of sensor unavailability map - # matching data enables dead reckoning. - # This requires map matched latitude/longitude + # matching data enables dead reckoning. + # This requires map matched latitude/longitude # or heading data. uint8 flags2 # further information about navigation output # power safe mode state (Only for FW version >= 7.01; undefined otherwise) uint8 FLAGS2_PSM_STATE_MASK = 3 -uint8 PSM_STATE_ACQUISITION = 0 # ACQUISITION +uint8 PSM_STATE_ACQUISITION = 0 # ACQUISITION # [or when psm disabled] uint8 PSM_STATE_TRACKING = 1 # TRACKING uint8 PSM_STATE_POWER_OPTIMIZED_TRACKING = 2 # POWER OPTIMIZED TRACKING uint8 PSM_STATE_INACTIVE = 3 # INACTIVE -# Note that the spoofing state value only reflects the detector state for the -# current navigation epoch. As spoofing can be detected most easily at the -# transition from real signal to spoofing signal, this is also where the +# Note that the spoofing state value only reflects the detector state for the +# current navigation epoch. As spoofing can be detected most easily at the +# transition from real signal to spoofing signal, this is also where the # detector is triggered the most. I.e. a value of 1 - No spoofing indicated does # not mean that the receiver is not spoofed, it #simply states that the detector # was not triggered in this epoch. -uint8 FLAGS2_SPOOF_DET_STATE_MASK = 24 +uint8 FLAGS2_SPOOF_DET_STATE_MASK = 24 uint8 SPOOF_DET_STATE_UNKNOWN = 0 # Unknown or deactivated uint8 SPOOF_DET_STATE_NONE = 8 # No spoofing indicated uint8 SPOOF_DET_STATE_SPOOFING = 16 # Spoofing indicated diff --git a/ublox_msgs/msg/NavSVIN.msg b/ublox_msgs/msg/NavSVIN.msg index 93e6205c..1237fea2 100644 --- a/ublox_msgs/msg/NavSVIN.msg +++ b/ublox_msgs/msg/NavSVIN.msg @@ -1,6 +1,6 @@ # NAV-SVIN (0x01 0x3B) # Survey-in data -# +# # This message contains information about survey-in parameters. # Supported on: # - u-blox 8 / u-blox M8 with protocol version 20 (only with High Precision @@ -12,28 +12,28 @@ uint8 MESSAGE_ID = 59 uint8 version # Message version (0x00 for this version) uint8[3] reserved0 # Reserved -uint32 iTOW # GPS time of week of the navigation epoch [ms] +uint32 i_tow # GPS time of week of the navigation epoch [ms] uint32 dur # Passed survey-in observation time [s] -int32 meanX # Current survey-in mean position ECEF X coordinate [cm] -int32 meanY # Current survey-in mean position ECEF Y coordinate [cm] -int32 meanZ # Current survey-in mean position ECEF Z coordinate [cm] +int32 mean_x # Current survey-in mean position ECEF X coordinate [cm] +int32 mean_y # Current survey-in mean position ECEF Y coordinate [cm] +int32 mean_z # Current survey-in mean position ECEF Z coordinate [cm] -int8 meanXHP # Current high-precision survey-in mean position - # ECEF X coordinate. 0.1_mm +int8 mean_xhp # Current high-precision survey-in mean position + # ECEF X coordinate. 0.1_mm # Must be in the range -99..+99. # The current survey-in mean position ECEF X # coordinate, in units of cm, is given by # meanX + (0.01 * meanXHP) -int8 meanYHP # Current high-precision survey-in mean position - # ECEF Y coordinate. [0.1 mm] +int8 mean_yhp # Current high-precision survey-in mean position + # ECEF Y coordinate. [0.1 mm] # Must be in the range -99..+99. # The current survey-in mean position ECEF Y # coordinate, in units of cm, is given by # meanY + (0.01 * meanYHP) -int8 meanZHP # Current high-precision survey-in mean position - # ECEF Z coordinate. [0.1 mm] +int8 mean_zhp # Current high-precision survey-in mean position + # ECEF Z coordinate. [0.1 mm] # Must be in the range -99..+99. # The current survey-in mean position ECEF Z # coordinate, in units of cm, is given by @@ -41,11 +41,11 @@ int8 meanZHP # Current high-precision survey-in mean position uint8 reserved1 # Reserved -uint32 meanAcc # Current survey-in mean position accuracy [0.1 mm] +uint32 mean_acc # Current survey-in mean position accuracy [0.1 mm] uint32 obs # Number of position observations used during survey-in uint8 valid # Survey-in position validity flag, 1 = valid # otherwise 0 uint8 active # Survey-in in progress flag, 1 = in-progress # otherwise 0 -uint8[2] reserved3 # Reserved \ No newline at end of file +uint8[2] reserved3 # Reserved diff --git a/ublox_msgs/msg/NavSVINFO.msg b/ublox_msgs/msg/NavSVINFO.msg index b7d3bb52..8e4ebd30 100644 --- a/ublox_msgs/msg/NavSVINFO.msg +++ b/ublox_msgs/msg/NavSVINFO.msg @@ -5,11 +5,11 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 48 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint8 numCh # Number of channels +uint8 num_ch # Number of channels -uint8 globalFlags # Bitmask +uint8 global_flags # Bitmask # Chip Hardware generation flags uint8 CHIPGEN_ANTARIS = 0 # Antaris, Antaris 4 uint8 CHIPGEN_UBLOX5 = 1 # u-blox 5 @@ -19,4 +19,4 @@ uint8 CHIPGEN_UBLOX8 = 4 # u-blox 8 / u-blox M8 uint16 reserved2 # Reserved -NavSVINFO_SV[] sv +NavSVINFOSV[] sv diff --git a/ublox_msgs/msg/NavSVINFO_SV.msg b/ublox_msgs/msg/NavSVINFOSV.msg similarity index 93% rename from ublox_msgs/msg/NavSVINFO_SV.msg rename to ublox_msgs/msg/NavSVINFOSV.msg index befaed6e..d6675aa0 100644 --- a/ublox_msgs/msg/NavSVINFO_SV.msg +++ b/ublox_msgs/msg/NavSVINFOSV.msg @@ -6,20 +6,20 @@ uint8 svid # Satellite ID uint8 flags # Bitmask uint8 FLAGS_SV_USED = 1 # SV is used for navigation -uint8 FLAGS_DIFF_CORR = 2 # Differential correction data +uint8 FLAGS_DIFF_CORR = 2 # Differential correction data # is available for this SV -uint8 FLAGS_ORBIT_AVAIL = 4 # Orbit information is available for +uint8 FLAGS_ORBIT_AVAIL = 4 # Orbit information is available for # this SV (Ephemeris or Almanach) uint8 FLAGS_ORBIT_EPH = 8 # Orbit information is Ephemeris -uint8 FLAGS_UNHEALTHY = 16 # SV is unhealthy / shall not be +uint8 FLAGS_UNHEALTHY = 16 # SV is unhealthy / shall not be # used uint8 FLAGS_ORBIT_ALM = 32 # Orbit information is Almanac Plus -uint8 FLAGS_ORBIT_AOP = 64 # Orbit information is AssistNow +uint8 FLAGS_ORBIT_AOP = 64 # Orbit information is AssistNow # Autonomous uint8 FLAGS_SMOOTHED = 128 # Carrier smoothed pseudorange used uint8 quality # Bitfield -# qualityInd: Signal Quality indicator (range 0..7). The following list shows +# qualityInd: Signal Quality indicator (range 0..7). The following list shows # the meaning of the different QI values: # Note: Since IMES signals are not time synchronized, a channel tracking an IMES # signal can never reach a quality indicator value of higher than 3. @@ -28,14 +28,14 @@ uint8 QUALITY_SEARCHING = 1 # Channel is searching uint8 QUALITY_ACQUIRED = 2 # Signal acquired uint8 QUALITY_DETECTED = 3 # Signal detected but unusable uint8 QUALITY_CODE_LOCK = 4 # Code Lock on Signal -uint8 QUALITY_CODE_AND_CARRIER_LOCKED1 = 5 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED1 = 5 # Code and Carrier locked # and time synchronized -uint8 QUALITY_CODE_AND_CARRIER_LOCKED2 = 6 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED2 = 6 # Code and Carrier locked # and time synchronized -uint8 QUALITY_CODE_AND_CARRIER_LOCKED3 = 7 # Code and Carrier locked +uint8 QUALITY_CODE_AND_CARRIER_LOCKED3 = 7 # Code and Carrier locked # and time synchronized uint8 cno # Carrier to Noise Ratio (Signal Strength) [dBHz] int8 elev # Elevation in integer degrees [deg] int16 azim # Azimuth in integer degrees [deg] -int32 prRes # Pseudo range residual in centimetres [cm] +int32 pr_res # Pseudo range residual in centimetres [cm] diff --git a/ublox_msgs/msg/NavTIMEGPS.msg b/ublox_msgs/msg/NavTIMEGPS.msg index ec69e993..9d2136d7 100644 --- a/ublox_msgs/msg/NavTIMEGPS.msg +++ b/ublox_msgs/msg/NavTIMEGPS.msg @@ -5,16 +5,16 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 32 -uint32 iTOW # GPS Millisecond time of week [ms] -int32 fTOW # Fractional Nanoseconds remainder of rounded +uint32 i_tow # GPS Millisecond time of week [ms] +int32 f_tow # Fractional Nanoseconds remainder of rounded # ms above, range -500000 .. 500000 [ns] int16 week # GPS week (GPS time) -int8 leapS # Leap Seconds (GPS-UTC) [s] +int8 leap_s # Leap Seconds (GPS-UTC) [s] uint8 valid # Validity Flags uint8 VALID_TOW = 1 # Valid Time of Week uint8 VALID_WEEK = 2 # Valid Week Number uint8 VALID_LEAP_S = 4 # Valid Leap Seconds -uint32 tAcc # Time Accuracy Estimate [ns] +uint32 t_acc # Time Accuracy Estimate [ns] diff --git a/ublox_msgs/msg/NavTIMEUTC.msg b/ublox_msgs/msg/NavTIMEUTC.msg index f9d57171..3ab44017 100644 --- a/ublox_msgs/msg/NavTIMEUTC.msg +++ b/ublox_msgs/msg/NavTIMEUTC.msg @@ -5,16 +5,16 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 33 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -uint32 tAcc # Time Accuracy Estimate [ns] +uint32 t_acc # Time Accuracy Estimate [ns] int32 nano # Fraction of second, range -1e9 .. 1e9 (UTC) [ns] uint16 year # Year, range 1999..2099 (UTC) [y] uint8 month # Month, range 1..12 (UTC) [month] uint8 day # Day of Month, range 1..31 (UTC) [d] uint8 hour # Hour of Day, range 0..23 (UTC) [h] uint8 min # Minute of Hour, range 0..59 (UTC) [min] -uint8 sec # Seconds of Minute, range 0..60 (UTC) [s] (60 for +uint8 sec # Seconds of Minute, range 0..60 (UTC) [s] (60 for # leap second) uint8 valid # Validity Flags @@ -24,12 +24,12 @@ uint8 VALID_UTC = 4 # Valid Leap Seconds, i.e. Leap Seconds already know uint8 VALID_UTC_STANDARD_MASK = 240 # UTC standard Identifier Bit mask: uint8 UTC_STANDARD_NOT_AVAILABLE = 0 # Information not available uint8 UTC_STANDARD_CRL = 16 # Communications Research Labratory -uint8 UTC_STANDARD_NIST = 32 # National Institute of Standards and +uint8 UTC_STANDARD_NIST = 32 # National Institute of Standards and # Technology (NIST) uint8 UTC_STANDARD_USNO = 48 # U.S. Naval Observatory (USNO) -uint8 UTC_STANDARD_BIPM = 64 # International Bureau of Weights and +uint8 UTC_STANDARD_BIPM = 64 # International Bureau of Weights and # Measures (BIPM) uint8 UTC_STANDARD_EL = 80 # European Laboratory (tbd) uint8 UTC_STANDARD_SU = 96 # Former Soviet Union (SU) uint8 UTC_STANDARD_NTSC = 112 # National Time Service Center, China -uint8 UTC_STANDARD_UNKNOWN = 240 \ No newline at end of file +uint8 UTC_STANDARD_UNKNOWN = 240 diff --git a/ublox_msgs/msg/NavVELECEF.msg b/ublox_msgs/msg/NavVELECEF.msg index 52f5ceb6..bf11daa1 100644 --- a/ublox_msgs/msg/NavVELECEF.msg +++ b/ublox_msgs/msg/NavVELECEF.msg @@ -8,9 +8,9 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 17 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -int32 ecefVX # ECEF X velocity [cm/s] -int32 ecefVY # ECEF Y velocity [cm/s] -int32 ecefVZ # ECEF Z velocity [cm/s] -uint32 sAcc # Speed Accuracy Estimate [cm/s] +int32 ecef_vx # ECEF X velocity [cm/s] +int32 ecef_vy # ECEF Y velocity [cm/s] +int32 ecef_vz # ECEF Z velocity [cm/s] +uint32 s_acc # Speed Accuracy Estimate [cm/s] diff --git a/ublox_msgs/msg/NavVELNED.msg b/ublox_msgs/msg/NavVELNED.msg index 8796e8f6..e63325ea 100644 --- a/ublox_msgs/msg/NavVELNED.msg +++ b/ublox_msgs/msg/NavVELNED.msg @@ -8,13 +8,13 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 18 -uint32 iTOW # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] -int32 velN # NED north velocity [cm/s] -int32 velE # NED east velocity [cm/s] -int32 velD # NED down velocity [cm/s] +int32 vel_n # NED north velocity [cm/s] +int32 vel_e # NED east velocity [cm/s] +int32 vel_d # NED down velocity [cm/s] uint32 speed # Speed (3-D) [cm/s] -uint32 gSpeed # Ground Speed (2-D) [cm/s] +uint32 g_speed # Ground Speed (2-D) [cm/s] int32 heading # Heading of motion 2-D [deg / 1e-5] -uint32 sAcc # Speed Accuracy Estimate [cm/s] -uint32 cAcc # Course / Heading Accuracy Estimate [deg / 1e-5] +uint32 s_acc # Speed Accuracy Estimate [cm/s] +uint32 c_acc # Course / Heading Accuracy Estimate [deg / 1e-5] diff --git a/ublox_msgs/msg/RxmRAW.msg b/ublox_msgs/msg/RxmRAW.msg index b8e7581c..c966f598 100644 --- a/ublox_msgs/msg/RxmRAW.msg +++ b/ublox_msgs/msg/RxmRAW.msg @@ -2,20 +2,20 @@ # Raw Measurement Data # # Supported up to ublox 7 firmware. See RxmRAWX for ublox 8 -# This message contains all information needed to be able to generate a RINEX +# This message contains all information needed to be able to generate a RINEX # observation file. -# This message outputs pseudorange, doppler and carrier phase measurements for -# GPS satellites once signals have been synchronised. No other GNSS types are +# This message outputs pseudorange, doppler and carrier phase measurements for +# GPS satellites once signals have been synchronised. No other GNSS types are # currently supported. -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 16 -int32 rcvTOW # Measurement time of week in receiver local time [s] +int32 rcv_tow # Measurement time of week in receiver local time [s] int16 week # Measurement week number in receiver local time [weeks] -uint8 numSV # # of satellites following +uint8 num_sv # # of satellites following uint8 reserved1 # Reserved -RxmRAW_SV[] sv # numSV times +RxmRAWSV[] sv # numSV times diff --git a/ublox_msgs/msg/RxmRAW_SV.msg b/ublox_msgs/msg/RxmRAWSV.msg similarity index 61% rename from ublox_msgs/msg/RxmRAW_SV.msg rename to ublox_msgs/msg/RxmRAWSV.msg index 70c3e604..954af99a 100644 --- a/ublox_msgs/msg/RxmRAW_SV.msg +++ b/ublox_msgs/msg/RxmRAWSV.msg @@ -1,15 +1,15 @@ # see message RxmRAW # -float64 cpMes # Carrier phase measurement [L1 cycles] -float64 prMes # Pseudorange measurement [m] -float32 doMes # Doppler measurement [Hz] +float64 cp_mes # Carrier phase measurement [L1 cycles] +float64 pr_mes # Pseudorange measurement [m] +float32 do_mes # Doppler measurement [Hz] uint8 sv # Space Vehicle Number -int8 mesQI # Nav Measurements Quality Indicator +int8 mes_qi # Nav Measurements Quality Indicator # >=4 : PR+DO OK # >=5 : PR+DO+CP OK - # <6 : likely loss of carrier lock in previous + # <6 : likely loss of carrier lock in previous # interval int8 cno # Signal strength C/No. [dbHz] uint8 lli # Loss of lock indicator (RINEX definition) diff --git a/ublox_msgs/msg/RxmRAWX.msg b/ublox_msgs/msg/RxmRAWX.msg index 7fe3c2bd..a985913c 100644 --- a/ublox_msgs/msg/RxmRAWX.msg +++ b/ublox_msgs/msg/RxmRAWX.msg @@ -6,34 +6,34 @@ # This message contains pseudorange, Doppler, carrier phase, phase lock and # signal quality information for GNSS satellites once signals have been # synchronized. This message supports all active GNSS. -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 21 -float64 rcvTOW # Measurement time of week in receiver local time [s] +float64 rcv_tow # Measurement time of week in receiver local time [s] # approximately aligned to the GPS time system. The # receiver local time of week number and leap second - # information can be used to translate the time to other + # information can be used to translate the time to other # time systems. More information about the difference in - # time systems can be found in RINEX 3 documentation. + # time systems can be found in RINEX 3 documentation. # For a receiver operating in GLONASS only mode, UTC - # time can be determined by subtracting the leapS field + # time can be determined by subtracting the leapS field # from GPS time regardless of whether the GPS leap - # seconds are valid. + # seconds are valid. uint16 week # GPS week number in receiver local time. [weeks] -int8 leapS # GPS leap seconds (GPS-UTC). [s] +int8 leap_s # GPS leap seconds (GPS-UTC). [s] # This field represents the receiver's best knowledge of # the leap seconds offset. A flag is given in the - # recStat bitfield to indicate if the leap seconds + # recStat bitfield to indicate if the leap seconds # are known. -uint8 numMeas # # of measurements to follow -uint8 recStat # Receiver tracking status bitfield +uint8 num_meas # # of measurements to follow +uint8 rec_stat # Receiver tracking status bitfield uint8 REC_STAT_LEAP_SEC = 1 # Leap seconds have been determined -uint8 REC_STAT_CLK_RESET = 2 # Clock reset applied. Typically the receiver +uint8 REC_STAT_CLK_RESET = 2 # Clock reset applied. Typically the receiver # clock is changed in increments of integer # milliseconds. uint8 version # Message version (0x01 for this version). uint8[2] reserved1 # Reserved -RxmRAWX_Meas[] meas +RxmRAWXMeas[] meas diff --git a/ublox_msgs/msg/RxmRAWX_Meas.msg b/ublox_msgs/msg/RxmRAWXMeas.msg similarity index 63% rename from ublox_msgs/msg/RxmRAWX_Meas.msg rename to ublox_msgs/msg/RxmRAWXMeas.msg index 37ddac1d..80bc7060 100644 --- a/ublox_msgs/msg/RxmRAWX_Meas.msg +++ b/ublox_msgs/msg/RxmRAWXMeas.msg @@ -1,39 +1,39 @@ # see message RxmRAWX # -float64 prMes # Pseudorange measurement [m]. GLONASS inter frequency +float64 pr_mes # Pseudorange measurement [m]. GLONASS inter frequency # channel delays are compensated with an internal # calibration table. -float64 cpMes # Carrier phase measurement [L1 cycles]. The carrier +float64 cp_mes # Carrier phase measurement [L1 cycles]. The carrier # phase initial ambiguity is initialized using an # approximate value to make the magnitude of # the phase close to the pseudorange # measurement. Clock resets are applied to both # phase and code measurements in accordance # with the RINEX specification. -float32 doMes # Doppler measurement [Hz] (positive sign for +float32 do_mes # Doppler measurement [Hz] (positive sign for # approaching satellites) -uint8 gnssId # GNSS identifier (see CfgGNSS for constants) +uint8 gnss_id # GNSS identifier (see CfgGNSS for constants) -uint8 svId # Satellite identifier (see Satellite Numbering) +uint8 sv_id # Satellite identifier (see Satellite Numbering) uint8 reserved0 # Reserved -uint8 freqId # Only used for GLONASS: This is the frequency +uint8 freq_id # Only used for GLONASS: This is the frequency # slot + 7 (range from 0 to 13) -uint16 locktime # Carrier phase locktime counter [ms] +uint16 locktime # Carrier phase locktime counter [ms] # (maximum 64500 ms) -int8 cno # Carrier-to-noise density ratio (signal strength) +int8 cno # Carrier-to-noise density ratio (signal strength) # [dB-Hz] -uint8 prStdev # Estimated pseudorange measurement standard +uint8 pr_stdev # Estimated pseudorange measurement standard # deviation [m / 0.01*2^n] -uint8 cpStdev # Estimated carrier phase measurement standard +uint8 cp_stdev # Estimated carrier phase measurement standard # deviation (note a raw value of 0x0F indicates the # value is invalid) [cycles / 0.004] -uint8 doStdev # Estimated Doppler measurement standard deviation +uint8 do_stdev # Estimated Doppler measurement standard deviation # [Hz / 0.002*2^n] -uint8 trkStat # Tracking status bitfield +uint8 trk_stat # Tracking status bitfield uint8 TRK_STAT_PR_VALID = 1 # Pseudorange valid uint8 TRK_STAT_CP_VALID = 2 # Carrier phase valid uint8 TRK_STAT_HALF_CYC = 4 # Half cycle valid diff --git a/ublox_msgs/msg/RxmRTCM.msg b/ublox_msgs/msg/RxmRTCM.msg index 4201d798..b0e97515 100644 --- a/ublox_msgs/msg/RxmRTCM.msg +++ b/ublox_msgs/msg/RxmRTCM.msg @@ -4,18 +4,18 @@ # Output upon processing of an RTCM input message # Supported on: # - u-blox 8 / u-blox M8 from protocol version 20.01 up to version 23.01 -# +# uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 50 uint8 version # Message version (0x02 for this version) uint8 flags # RTCM input status flags -uint8 FLAGS_CRC_FAILED = 1 # 0 when RTCM message received and passed CRC +uint8 FLAGS_CRC_FAILED = 1 # 0 when RTCM message received and passed CRC # check, 1 when failed in which case refStation # and msgType might be corrupted and misleading uint8[2] reserved0 # Reserved -uint16 refStation # Reference station ID -uint16 msgType # Message type +uint16 ref_station # Reference station ID +uint16 msg_type # Message type diff --git a/ublox_msgs/msg/RxmSFRBX.msg b/ublox_msgs/msg/RxmSFRBX.msg index 9cb2bef1..bf9351c2 100644 --- a/ublox_msgs/msg/RxmSFRBX.msg +++ b/ublox_msgs/msg/RxmSFRBX.msg @@ -1,7 +1,7 @@ # RXM-SFRB (0x02 0x13) # Subframe Buffer # -# This message reports a complete subframe of broadcast navigation data decoded +# This message reports a complete subframe of broadcast navigation data decoded # from a single signal. The number of data words reported in each message # depends on the nature of the signal. See the section on Broadcast Navigation # Data for further details. @@ -10,13 +10,13 @@ uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 19 -uint8 gnssId # GNSS identifier (see Cfg GNSS for constants) +uint8 gnss_id # GNSS identifier (see Cfg GNSS for constants) -uint8 svId # Satellite identifier within corresponding GNSS system +uint8 sv_id # Satellite identifier within corresponding GNSS system uint8 reserved0 # Reserved -uint8 freqId # Only used for GLONASS: This is the frequency +uint8 freq_id # Only used for GLONASS: This is the frequency # slot + 7 (range from 0 to 13) -uint8 numWords # The number of data words contained in this message (up +uint8 num_words # The number of data words contained in this message (up # to 10, for currently supported signals) uint8 chn # The tracking channel number the message was received # on @@ -25,4 +25,4 @@ uint8 reserved1 # Reserved # Start of repeated block (numWords times) uint32[] dwrd # The data words -# End of repeated block \ No newline at end of file +# End of repeated block diff --git a/ublox_msgs/msg/RxmSVSI.msg b/ublox_msgs/msg/RxmSVSI.msg index b163c7f8..f686def0 100644 --- a/ublox_msgs/msg/RxmSVSI.msg +++ b/ublox_msgs/msg/RxmSVSI.msg @@ -9,10 +9,10 @@ uint8 CLASS_ID = 2 uint8 MESSAGE_ID = 32 -int32 iTOW # GPS time of week of the navigation epoch [ms] +int32 i_tow # GPS time of week of the navigation epoch [ms] int16 week # GPS week number of the navigation epoch [weeks] -uint8 numVis # Number of visible satellites -uint8 numSV # Number of per-SV data blocks following +uint8 num_vis # Number of visible satellites +uint8 num_sv # Number of per-SV data blocks following -RxmSVSI_SV[] sv +RxmSVSISV[] sv diff --git a/ublox_msgs/msg/RxmSVSISV.msg b/ublox_msgs/msg/RxmSVSISV.msg new file mode 100644 index 00000000..aa37b479 --- /dev/null +++ b/ublox_msgs/msg/RxmSVSISV.msg @@ -0,0 +1,22 @@ +# see message RxmSVSI +# + +uint8 svid # Satellite ID + +uint8 sv_flag # Information Flags +uint8 FLAG_URA_MASK = 15 # Figure of Merit (URA) range 0..15 +uint8 FLAG_HEALTHY = 16 # SV healthy flag +uint8 FLAG_EPH_VAL = 32 # Ephemeris valid +uint8 FLAG_ALM_VAL = 64 # Almanac valid +uint8 FLAG_NOT_AVAIL = 128 # SV not available + +int16 azim # Azimuth +int8 elev # Elevation + +uint8 age # Age of Almanac and Ephemeris +uint8 AGE_ALM_MASK = 15 # Age of ALM in days offset by 4 + # i.e. the reference time may be in the future: + # ageOfAlm = (age & 0x0f) - 4 +uint8 AGE_EPH_MASK = 240 # Age of EPH in hours offset by 4. + # i.e. the reference time may be in the future: + # ageOfEph = ((age & 0xf0) >> 4) - 4 diff --git a/ublox_msgs/msg/TimTM2.msg b/ublox_msgs/msg/TimTM2.msg index 2ff4a065..83582704 100644 --- a/ublox_msgs/msg/TimTM2.msg +++ b/ublox_msgs/msg/TimTM2.msg @@ -1,13 +1,13 @@ # TIM-TM2 (0x0D, 0x03) -# Time mark data +# Time mark data # # Description for details. -# +# # Supported on: # - u-blox 8 / u-blox M8 with protocol version 22 (only with Timing Products) # -uint8 CLASS_ID = 13 +uint8 CLASS_ID = 13 uint8 MESSAGE_ID = 3 uint8 ch # Channel (i.e. EXTINT) upon which the pulse was measured @@ -22,11 +22,11 @@ uint8 FLAGS_UTC_AVAIL = 32 # 0 = utc not available, 1 = utc available uint8 FLAGS_TIME_VALID = 64 # 0 = time is not valid, 1 time is valid uint8 FLAGS_NEWRISINGEDGE = 128 # new rising edge detected -uint16 risingEdgeCount # rising edge count -uint16 wnR # week number of last rising edge -uint16 wnF # week number of last falling edge -uint32 towMsR # Tow of rising edge -uint32 towSubMsR # Millisecond Fraction of Tow of rising edge in nanoseconds -uint32 towMsF # tow of falling edge -uint32 towSubMsF # millisecond fraction of tow of falling edge in nanoseconds -uint32 accEst # Accuracy estimate +uint16 rising_edge_count # rising edge count +uint16 wn_r # week number of last rising edge +uint16 wn_f # week number of last falling edge +uint32 tow_ms_r # Tow of rising edge +uint32 tow_sub_ms_r # Millisecond Fraction of Tow of rising edge in nanoseconds +uint32 tow_ms_f # tow of falling edge +uint32 tow_sub_ms_f # millisecond fraction of tow of falling edge in nanoseconds +uint32 acc_est # Accuracy estimate diff --git a/ublox_msgs/msg/UpdSOS.msg b/ublox_msgs/msg/UpdSOS.msg index 139b3a2f..236746e4 100644 --- a/ublox_msgs/msg/UpdSOS.msg +++ b/ublox_msgs/msg/UpdSOS.msg @@ -8,18 +8,18 @@ uint8 CLASS_ID = 9 uint8 MESSAGE_ID = 20 uint8 cmd # Command -# The host can send this message in order to save part of the BBR memory in a -# file in flash file system. The feature is designed in order to emulate the -# presence of the backup battery even if it is not present; the host can issue -# the save on shutdown command before switching off the device supply. It is -# recommended to issue a GNSS stop command before, in order to keep the BBR +# The host can send this message in order to save part of the BBR memory in a +# file in flash file system. The feature is designed in order to emulate the +# presence of the backup battery even if it is not present; the host can issue +# the save on shutdown command before switching off the device supply. It is +# recommended to issue a GNSS stop command before, in order to keep the BBR # memory content consistent. uint8 CMD_FLASH_BACKUP_CREATE = 0 # Create Backup File in Flash -# The host can send this message in order to erase the backup file present in -# flash. It is recommended that the clear operation is issued after the host has -# received the notification that the memory has been restored after a reset. -# Alternatively the host can parse the startup string 'Restored data saved on +# The host can send this message in order to erase the backup file present in +# flash. It is recommended that the clear operation is issued after the host has +# received the notification that the memory has been restored after a reset. +# Alternatively the host can parse the startup string 'Restored data saved on # shutdown' or poll the UBX-UPD-SOS message for getting the status. uint8 CMD_FLASH_BACKUP_CLEAR = 1 # Clear Backup File in Flash -uint8[3] reserved1 # Reserved \ No newline at end of file +uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/msg/UpdSOS_Ack.msg b/ublox_msgs/msg/UpdSOSAck.msg similarity index 90% rename from ublox_msgs/msg/UpdSOS_Ack.msg rename to ublox_msgs/msg/UpdSOSAck.msg index 82584431..680cf091 100644 --- a/ublox_msgs/msg/UpdSOS_Ack.msg +++ b/ublox_msgs/msg/UpdSOSAck.msg @@ -1,7 +1,7 @@ # UPD-SOS (0x09 0x14) # # Backup File Creation Acknowledge / System Restored from Backup -# +# # Firmware Supported on: # u-blox 8 / u-blox M8 from protocol version 15 up to version 23.01 # @@ -11,17 +11,17 @@ uint8 MESSAGE_ID = 20 uint8 cmd # Command uint8 CMD_BACKUP_CREATE_ACK = 2 # Backup File Creation Acknowledge - # The message is sent from the device as - # confirmation of creation of a backup file - # in flash. The host can safely shut down the + # The message is sent from the device as + # confirmation of creation of a backup file + # in flash. The host can safely shut down the # device after received this message. uint8 CMD_SYSTEM_RESTORED = 3 # System Restored from Backup - # The message is sent from the device to - # notify the host the BBR has been restored - # from a backup file in flash. The host - # should clear the backup file after - # receiving this message. If the UBX-UPD-SOS - # message is polled, this message will be + # The message is sent from the device to + # notify the host the BBR has been restored + # from a backup file in flash. The host + # should clear the backup file after + # receiving this message. If the UBX-UPD-SOS + # message is polled, this message will be # present. uint8[3] reserved0 # Reserved @@ -30,9 +30,9 @@ uint8 response # Response: uint8 BACKUP_CREATE_NACK = 0 # Not acknowledged uint8 BACKUP_CREATE_ACK = 1 # Acknowledged uint8 SYSTEM_RESTORED_RESPONSE_UNKNOWN = 0 # Unknown -uint8 SYSTEM_RESTORED_RESPONSE_FAILED = 1 # Failed restoring from backup +uint8 SYSTEM_RESTORED_RESPONSE_FAILED = 1 # Failed restoring from backup # file uint8 SYSTEM_RESTORED_RESPONSE_RESTORED = 2 # Restored from backup file uint8 SYSTEM_RESTORED_RESPONSE_NOT_RESTORED = 3 # Not restored (no backup) -uint8[3] reserved1 # Reserved \ No newline at end of file +uint8[3] reserved1 # Reserved diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 7c4e5d18..bcc60217 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -26,6 +26,8 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== +#include + #include template @@ -160,7 +162,7 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS); // SOS and SOS_Ack have the same message ID, but different lengths DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS_Ack); + ublox_msgs, UpdSOSAck); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, ublox_msgs, MonGNSS); From 5cf25d3006b8d8276ac751e069b2679a0b8fbce1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:20:25 -0500 Subject: [PATCH 006/152] Fully de-boostify callback.hpp Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 73036203..1fa9299c 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -31,13 +31,12 @@ #include #include +#include #include +#include #include #include -#include -#include -#include namespace ublox_gps { @@ -71,7 +70,7 @@ class CallbackHandler { template class CallbackHandler_ : public CallbackHandler { public: - typedef boost::function Callback; //!< A callback function + typedef std::function Callback; //!< A callback function /** * @brief Initialize the Callback Handler with a callback function @@ -135,7 +134,7 @@ class CallbackHandlers { CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler))); + std::shared_ptr(handler))); } /** @@ -154,7 +153,7 @@ class CallbackHandlers { CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, message_id), - boost::shared_ptr(handler))); + std::shared_ptr(handler))); } /** @@ -167,8 +166,9 @@ class CallbackHandlers { Callbacks::key_type key = std::make_pair(reader.classId(), reader.messageId()); for (Callbacks::iterator callback = callbacks_.lower_bound(key); - callback != callbacks_.upper_bound(key); ++callback) + callback != callbacks_.upper_bound(key); ++callback) { callback->second->handle(reader); + } } /** @@ -184,7 +184,7 @@ class CallbackHandlers { CallbackHandler_* handler = new CallbackHandler_(); Callbacks::iterator callback = callbacks_.insert( (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - boost::shared_ptr(handler)))); + std::shared_ptr(handler)))); callback_mutex_.unlock(); // Wait for the message @@ -214,8 +214,9 @@ class CallbackHandlers { // Print the received bytes std::ostringstream oss; for (ublox::Reader::iterator it = reader.pos(); - it != reader.pos() + reader.length() + 8; ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; + it != reader.pos() + reader.length() + 8; ++it) { + oss << std::hex << static_cast(*it) << " "; + } ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, oss.str().c_str()); } @@ -230,7 +231,7 @@ class CallbackHandlers { private: typedef std::multimap, - boost::shared_ptr > Callbacks; + std::shared_ptr > Callbacks; // Call back handlers for u-blox messages Callbacks callbacks_; From 65b21e590e444138deabadca60c0c90716480330 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:31:30 -0500 Subject: [PATCH 007/152] Remove some uses of boost from async_worker.hpp Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 49 +++++++++++--------- ublox_gps/include/ublox_gps/callback.hpp | 1 + ublox_gps/include/ublox_gps/gps.hpp | 4 +- ublox_gps/src/gps.cpp | 22 +++++---- 4 files changed, 42 insertions(+), 34 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 2ac6fb5e..47aae1a8 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -31,16 +31,15 @@ #include #include +#include #include +#include +#include #include #include #include -#include -#include -#include - #include "worker.hpp" @@ -60,8 +59,8 @@ class AsyncWorker : public Worker { * @param io_service the I/O service * @param buffer_size the size of the input and output buffers */ - AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, + AsyncWorker(std::shared_ptr stream, + std::shared_ptr io_service, std::size_t buffer_size = 8192); virtual ~AsyncWorker(); @@ -114,8 +113,8 @@ class AsyncWorker : public Worker { */ void doClose(); - boost::shared_ptr stream_; //!< The I/O stream - boost::shared_ptr io_service_; //!< The I/O service + std::shared_ptr stream_; //!< The I/O stream + std::shared_ptr io_service_; //!< The I/O service std::mutex read_mutex_; //!< Lock for the input buffer std::condition_variable read_condition_; @@ -127,7 +126,7 @@ class AsyncWorker : public Worker { std::condition_variable write_condition_; std::vector out_; //!< The output buffer - boost::shared_ptr background_thread_; //!< thread for the I/O + std::shared_ptr background_thread_; //!< thread for the I/O //!< service Callback read_callback_; //!< Callback function to handle received messages Callback write_callback_; //!< Callback function to handle raw data @@ -136,8 +135,8 @@ class AsyncWorker : public Worker { }; template -AsyncWorker::AsyncWorker(boost::shared_ptr stream, - boost::shared_ptr io_service, +AsyncWorker::AsyncWorker(std::shared_ptr stream, + std::shared_ptr io_service, std::size_t buffer_size) : stopping_(false) { stream_ = stream; @@ -148,7 +147,7 @@ AsyncWorker::AsyncWorker(boost::shared_ptr stream, out_.reserve(buffer_size); io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_.reset(new boost::thread( + background_thread_.reset(new std::thread( boost::bind(&boost::asio::io_service::run, io_service_))); } @@ -163,7 +162,7 @@ template bool AsyncWorker::send(const unsigned char* data, const unsigned int size) { std::lock_guard lock(write_mutex_); - if(size == 0) { + if (size == 0) { ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); return true; } @@ -192,8 +191,9 @@ void AsyncWorker::doWrite() { // Print the data that was sent std::ostringstream oss; for (std::vector::iterator it = out_.begin(); - it != out_.end(); ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; + it != out_.end(); ++it) { + oss << std::hex << static_cast(*it) << " "; + } ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } // Clear the buffer & unlock @@ -226,27 +226,31 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); std::size_t raw_data_stream_size = bytes_transfered; - if (write_callback_) + if (write_callback_) { write_callback_(pRawDataStart, raw_data_stream_size); + } if (debug >= 4) { std::ostringstream oss; for (std::vector::iterator it = in_.begin() + in_buffer_size_ - bytes_transfered; - it != in_.begin() + in_buffer_size_; ++it) - oss << boost::format("%02x") % static_cast(*it) << " "; + it != in_.begin() + in_buffer_size_; ++it) { + oss << std::hex << static_cast(*it) << " "; + } ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - oss.str().c_str()); + oss.str().c_str()); } - if (read_callback_) + if (read_callback_) { read_callback_(in_.data(), in_buffer_size_); + } read_condition_.notify_all(); } - if (!stopping_) + if (!stopping_) { io_service_->post(boost::bind(&AsyncWorker::doRead, this)); + } } template @@ -255,9 +259,10 @@ void AsyncWorker::doClose() { stopping_ = true; boost::system::error_code error; stream_->close(error); - if(error) + if (error) { ROS_ERROR_STREAM( "Error while closing the AsyncWorker stream: " << error.message()); + } } template diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 1fa9299c..264fc6d3 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index f6d3b458..031f48c4 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -425,7 +425,7 @@ class Gps { * @brief Set the I/O worker * @param an I/O handler */ - void setWorker(const boost::shared_ptr& worker); + void setWorker(const std::shared_ptr& worker); /** * @brief Subscribe to ACK/NACK messages and UPD-SOS-ACK messages. @@ -463,7 +463,7 @@ class Gps { bool saveOnShutdown(); //! Processes I/O stream data - boost::shared_ptr worker_; + std::shared_ptr worker_; //! Whether or not the I/O port has been configured bool configured_; //! Whether or not to save Flash BBR on shutdown diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index a42a84de..8db66577 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -28,6 +28,8 @@ //============================================================================== #include +#include +#include #include #include @@ -47,7 +49,7 @@ Gps::Gps() : configured_(false), config_on_startup_flag_(true) { Gps::~Gps() { close(); } -void Gps::setWorker(const boost::shared_ptr& worker) { +void Gps::setWorker(const std::shared_ptr& worker) { if (worker_) { return; } @@ -111,9 +113,9 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { void Gps::initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) { port_ = port; - boost::shared_ptr io_service( + std::shared_ptr io_service( new boost::asio::io_service); - boost::shared_ptr serial( + std::shared_ptr serial( new boost::asio::serial_port(*io_service)); // open serial port @@ -141,7 +143,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, if (worker_) { return; } - setWorker(boost::shared_ptr( + setWorker(std::shared_ptr( new AsyncWorker(serial, io_service))); configured_ = false; @@ -176,9 +178,9 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } void Gps::resetSerial(std::string port) { - boost::shared_ptr io_service( + std::shared_ptr io_service( new boost::asio::io_service); - boost::shared_ptr serial( + std::shared_ptr serial( new boost::asio::serial_port(*io_service)); // open serial port @@ -195,7 +197,7 @@ void Gps::resetSerial(std::string port) { if (worker_) { return; } - setWorker(boost::shared_ptr( + setWorker(std::shared_ptr( new AsyncWorker(serial, io_service))); configured_ = false; @@ -221,7 +223,7 @@ void Gps::resetSerial(std::string port) { void Gps::initializeTcp(std::string host, std::string port) { host_ = host; port_ = port; - boost::shared_ptr io_service( + std::shared_ptr io_service( new boost::asio::io_service); boost::asio::ip::tcp::resolver::iterator endpoint; @@ -234,7 +236,7 @@ void Gps::initializeTcp(std::string host, std::string port) { port + " " + e.what()); } - boost::shared_ptr socket( + std::shared_ptr socket( new boost::asio::ip::tcp::socket(*io_service)); try { @@ -251,7 +253,7 @@ void Gps::initializeTcp(std::string host, std::string port) { if (worker_) { return; } - setWorker(boost::shared_ptr( + setWorker(std::shared_ptr( new AsyncWorker(socket, io_service))); } From 511161e6969bc96f94d61a2272135c097852776f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:34:21 -0500 Subject: [PATCH 008/152] Remove boost from worker.hpp Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/worker.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ublox_gps/include/ublox_gps/worker.hpp b/ublox_gps/include/ublox_gps/worker.hpp index d3964bf2..da1c87ce 100644 --- a/ublox_gps/include/ublox_gps/worker.hpp +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -30,9 +30,7 @@ #define UBLOX_GPS_WORKER_HPP #include - -#include -#include +#include namespace ublox_gps { @@ -41,7 +39,7 @@ namespace ublox_gps { */ class Worker { public: - typedef boost::function Callback; + typedef std::function Callback; virtual ~Worker() {} /** From d7728bc58fbc6d00220d09fe6876f9b8b2170dfc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:40:19 -0500 Subject: [PATCH 009/152] Remove more boost. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 10 ++++++---- ublox_gps/src/gps.cpp | 22 +++++++++++----------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 031f48c4..301ed4fb 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -30,6 +30,7 @@ #ifndef UBLOX_GPS_GPS_HPP #define UBLOX_GPS_GPS_HPP // STL +#include #include #include #include @@ -39,7 +40,6 @@ #include #include #include -#include // ROS #include // Other u-blox packages @@ -475,7 +475,7 @@ class Gps { //! The default timeout for ACK messages static const std::chrono::milliseconds default_timeout_; //! Stores last received ACK accessed by multiple threads - mutable boost::atomic ack_; + mutable std::atomic ack_; //! Callback handlers for u-blox messages CallbackHandlers callbacks_; @@ -522,7 +522,7 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Reset ack Ack ack; ack.type = WAIT; - ack_.store(ack, boost::memory_order_seq_cst); + ack_.store(ack, std::memory_order_seq_cst); // Encode the message std::vector out(kWriterSize); @@ -535,7 +535,9 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Send the message to the device worker_->send(out.data(), writer.end() - out.data()); - if (!wait) return true; + if (!wait) { + return true; + } // Wait for an acknowledgment and return whether or not it was received return waitForAcknowledge(default_timeout_, diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 8db66577..5df4bc96 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -28,12 +28,12 @@ //============================================================================== #include +#include #include #include #include #include -#include namespace ublox_gps { @@ -54,21 +54,21 @@ void Gps::setWorker(const std::shared_ptr& worker) { return; } worker_ = worker; - worker_->setCallback(boost::bind(&CallbackHandlers::readCallback, - &callbacks_, _1, _2)); + worker_->setCallback(std::bind(&CallbackHandlers::readCallback, + &callbacks_, std::placeholders::_1, std::placeholders::_2)); configured_ = static_cast(worker); } void Gps::subscribeAcks() { // Set NACK handler - subscribeId(boost::bind(&Gps::processNack, this, _1), + subscribeId(std::bind(&Gps::processNack, this, std::placeholders::_1), ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(boost::bind(&Gps::processAck, this, _1), + subscribeId(std::bind(&Gps::processAck, this, std::placeholders::_1), ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler subscribe( - boost::bind(&Gps::processUpdSosAck, this, _1)); + std::bind(&Gps::processUpdSosAck, this, std::placeholders::_1)); } void Gps::processAck(const ublox_msgs::Ack &m) { @@ -78,7 +78,7 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.class_id = m.cls_id; ack.msg_id = m.msg_id; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); + ack_.store(ack, std::memory_order_seq_cst); ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } @@ -90,7 +90,7 @@ void Gps::processNack(const ublox_msgs::Ack &m) { ack.class_id = m.cls_id; ack.msg_id = m.msg_id; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); + ack_.store(ack, std::memory_order_seq_cst); ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } @@ -101,7 +101,7 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { ack.class_id = m.CLASS_ID; ack.msg_id = m.MESSAGE_ID; // store the ack atomically - ack_.store(ack, boost::memory_order_seq_cst); + ack_.store(ack, std::memory_order_seq_cst); ROS_DEBUG_COND(ack.type == ACK && debug >= 2, "U-blox: received UPD SOS Backup ACK"); if (ack.type == NACK) { @@ -572,13 +572,13 @@ bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::system_clock::time_point wait_until = now + timeout; - Ack ack = ack_.load(boost::memory_order_seq_cst); + Ack ack = ack_.load(std::memory_order_seq_cst); while (std::chrono::system_clock::now() < wait_until && (ack.class_id != class_id || ack.msg_id != msg_id || ack.type == WAIT)) { worker_->wait(timeout); - ack = ack_.load(boost::memory_order_seq_cst); + ack = ack_.load(std::memory_order_seq_cst); } bool result = ack.type == ACK && ack.class_id == class_id From b83960dffb8a6a8b183bb14c21a780c791993e69 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:44:45 -0500 Subject: [PATCH 010/152] Remove boost from serialization. Signed-off-by: Chris Lalancette --- .../include/ublox/serialization.hpp | 53 ++++++++++++------- .../include/ublox/serialization_ros.hpp | 8 ++- 2 files changed, 38 insertions(+), 23 deletions(-) diff --git a/ublox_serialization/include/ublox/serialization.hpp b/ublox_serialization/include/ublox/serialization.hpp index dbb6caa8..1ff4f701 100644 --- a/ublox_serialization/include/ublox/serialization.hpp +++ b/ublox_serialization/include/ublox/serialization.hpp @@ -31,7 +31,6 @@ #include #include -#include #include #include @@ -74,7 +73,7 @@ struct Serializer { * @param message the output message */ static void read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message); + T &message); /** * @brief Get the length of the message payload in bytes. * @@ -82,8 +81,7 @@ struct Serializer { * @param message the message to get the length of * @return the length of the message in bytes. */ - static uint32_t serializedLength( - typename boost::call_traits::param_type message); + static uint32_t serializedLength(const T &message); /** * @brief Encode the message payload as a byte array. @@ -91,8 +89,7 @@ struct Serializer { * @param size the length of the buffer * @param message the output message */ - static void write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message); + static void write(uint8_t *data, uint32_t size, const T &message); }; /** @@ -184,13 +181,16 @@ class Reader { */ iterator search() { - if (found_) next(); + if (found_) { + next(); + } // Search for a message header for( ; count_ > 0; --count_, ++data_) { if (data_[0] == options_.sync_a && - (count_ == 1 || data_[1] == options_.sync_b)) + (count_ == 1 || data_[1] == options_.sync_b)) { break; + } } return data_; @@ -202,15 +202,22 @@ class Reader { */ bool found() { - if (found_) return true; + if (found_) { + return true; + } // Verify message is long enough to have sync chars, id, length & checksum - if (count_ < options_.wrapper_length()) return false; + if (count_ < options_.wrapper_length()) { + return false; + } // Verify the header bits - if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) + if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) { return false; + } // Verify that the buffer length is long enough based on the received // message length - if (count_ < length() + options_.wrapper_length()) return false; + if (count_ < length() + options_.wrapper_length()) { + return false; + } found_ = true; return true; @@ -273,11 +280,17 @@ class Reader { * @param search whether or not to skip to the next message in the buffer */ template - bool read(typename boost::call_traits::reference message, + bool read(T &message, bool search = false) { - if (search) this->search(); - if (!found()) return false; - if (!Message::canDecode(classId(), messageId())) return false; + if (search) { + this->search(); + } + if (!found()) { + return false; + } + if (!Message::canDecode(classId(), messageId())) { + return false; + } uint16_t chk; if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { @@ -298,7 +311,9 @@ class Reader { */ template bool hasType() { - if (!found()) return false; + if (!found()) { + return false; + } return Message::canDecode(classId(), messageId()); } @@ -308,7 +323,9 @@ class Reader { * ID */ bool isMessage(uint8_t class_id, uint8_t message_id) { - if (!found()) return false; + if (!found()) { + return false; + } return (classId() == class_id && messageId() == message_id); } diff --git a/ublox_serialization/include/ublox/serialization_ros.hpp b/ublox_serialization/include/ublox/serialization_ros.hpp index d0ff6aa4..5eeca7fb 100644 --- a/ublox_serialization/include/ublox/serialization_ros.hpp +++ b/ublox_serialization/include/ublox/serialization_ros.hpp @@ -38,20 +38,18 @@ namespace ublox { template void Serializer::read(const uint8_t *data, uint32_t count, - typename boost::call_traits::reference message) { + T &message) { ros::serialization::IStream stream(const_cast(data), count); ros::serialization::Serializer::read(stream, message); } template -uint32_t Serializer::serializedLength( - typename boost::call_traits::param_type message) { +uint32_t Serializer::serializedLength(const T &message) { return ros::serialization::Serializer::serializedLength(message); } template -void Serializer::write(uint8_t *data, uint32_t size, - typename boost::call_traits::param_type message) { +void Serializer::write(uint8_t *data, uint32_t size, const T &message) { ros::serialization::OStream stream(data, size); ros::serialization::Serializer::write(stream, message); } From 3b4080286d8b901fdfaea4837861e5d6b94e4767 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:50:19 -0500 Subject: [PATCH 011/152] Remove a bunch of boost from node.cpp Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 171 +++++++++++++++++++++-------------------- 1 file changed, 89 insertions(+), 82 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 61a3333d..78eeb3c7 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -27,11 +27,13 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include "ublox_gps/node.hpp" #include +#include #include #include +#include "ublox_gps/node.hpp" + using namespace ublox_node; // @@ -263,70 +265,70 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) - gps.subscribe(boost::bind( - publish, _1, "navstatus"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navstatus"), kSubscribeRate); nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) - gps.subscribe(boost::bind( - publish, _1, "navposecef"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navposecef"), kSubscribeRate); nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) - gps.subscribe(boost::bind( - publish, _1, "navclock"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navclock"), kSubscribeRate); // INF messages nh->param("inf/debug", enabled["inf_debug"], false); if (enabled["inf_debug"]) gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); nh->param("inf/error", enabled["inf_error"], enabled["inf"]); if (enabled["inf_error"]) gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); if (enabled["inf_notice"]) gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); nh->param("inf/test", enabled["inf_test"], enabled["inf"]); if (enabled["inf_test"]) gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); if (enabled["inf_warning"]) gps.subscribeId( - boost::bind(&UbloxNode::printInf, this, _1, + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) - gps.subscribe(boost::bind( - publish, _1, "aidalm"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "aidalm"), kSubscribeRate); nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) - gps.subscribe(boost::bind( - publish, _1, "aideph"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "aideph"), kSubscribeRate); nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) - gps.subscribe(boost::bind( - publish, _1, "aidhui"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "aidhui"), kSubscribeRate); for(int i = 0; i < components_.size(); i++) components_[i]->subscribe(); @@ -541,7 +543,7 @@ void UbloxNode::initializeIo() { // raw data stream logging if (rawDataStreamPa_.isEnabled()) { gps.setRawDataCallback( - boost::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, _1, _2)); + std::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); rawDataStreamPa_.initialize(); } } @@ -658,27 +660,27 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate); - gps.subscribe(boost::bind( + gps.subscribe(std::bind( + &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), kSubscribeRate); + gps.subscribe(std::bind( // Subscribe to Nav SOL - &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate); + &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav VELNED - gps.subscribe(boost::bind( - &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navsvinfo"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "monhw"), kSubscribeRate); } void UbloxFirmware6::fixDiagnostic( @@ -744,10 +746,11 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_.longitude = m.lon * 1e-7; fix_.altitude = m.height * 1e-3; - if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) + if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) { fix_.status.status = fix_.status.STATUS_FIX; - else + } else { fix_.status.status = fix_.status.STATUS_NO_FIX; + } // Convert from mm to m const double var_h = pow(m.h_acc / 1000.0, 2); @@ -978,22 +981,22 @@ void UbloxFirmware7::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps.subscribe(boost::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, _1), + gps.subscribe(std::bind( + &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) - gps.subscribe(boost::bind( - publish, _1, "navsvinfo"), + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navsvinfo"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "monhw"), kSubscribeRate); } // @@ -1212,25 +1215,25 @@ void UbloxFirmware8::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT gps.subscribe( - boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate); + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) - gps.subscribe(boost::bind( - publish, _1, "navsat"), kNavSvInfoSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navsat"), kNavSvInfoSubscribeRate); // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) - gps.subscribe(boost::bind( - publish, _1, "monhw"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "monhw"), kSubscribeRate); // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmrtcm"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmrtcm"), kSubscribeRate); } // @@ -1243,26 +1246,26 @@ void RawDataProduct::subscribe() { // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmraw"), kSubscribeRate); // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmsfrb"), kSubscribeRate); // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) - gps.subscribe(boost::bind( - publish, _1, "rxmeph"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmeph"), kSubscribeRate); // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) - gps.subscribe(boost::bind( - publish, _1, "rxmalm"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmalm"), kSubscribeRate); } void RawDataProduct::initializeRosDiagnostics() { @@ -1304,41 +1307,41 @@ void AdrUdrProduct::subscribe() { // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) - gps.subscribe(boost::bind( - publish, _1, "navatt"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "navatt"), kSubscribeRate); // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) - gps.subscribe(boost::bind( - publish, _1, "esfins"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "esfins"), kSubscribeRate); // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) - gps.subscribe(boost::bind( - publish, _1, "esfmeas"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "esfmeas"), kSubscribeRate); // also publish sensor_msgs::Imu - gps.subscribe(boost::bind( - &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), kSubscribeRate); // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) - gps.subscribe(boost::bind( - publish, _1, "esfraw"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "esfraw"), kSubscribeRate); // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) - gps.subscribe(boost::bind( - publish, _1, "esfstatus"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "esfstatus"), kSubscribeRate); // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) - gps.subscribe(boost::bind( - publish, _1, "hnrpvt"), kSubscribeRate); + gps.subscribe(std::bind( + publish, std::placeholders::_1, "hnrpvt"), kSubscribeRate); } void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { @@ -1547,8 +1550,8 @@ void HpgRefProduct::subscribe() { // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In - gps.subscribe(boost::bind( - &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), kSubscribeRate); } void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { @@ -1652,8 +1655,8 @@ void HpgRovProduct::subscribe() { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); } void HpgRovProduct::initializeRosDiagnostics() { @@ -1718,8 +1721,8 @@ void HpPosRecProduct::subscribe() { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(boost::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); // Whether to publish the Heading info from Nav Relative Position NED nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); @@ -1770,11 +1773,13 @@ void TimProduct::getRosParams() { bool TimProduct::configureUblox() { uint8_t r = 1; // Configure the reciever - if(!gps.setUTCtime()) + if (!gps.setUTCtime()) { throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); + } - if(!gps.setTimtm2(r)) + if (!gps.setTimtm2(r)) { throw std::runtime_error(std::string("Failed to Configure TIM Product")); + } return true; } @@ -1785,22 +1790,24 @@ void TimProduct::subscribe() { // Subscribe to TIM-TM2 messages (Time mark messages) nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - gps.subscribe(boost::bind( - &TimProduct::callbackTimTM2, this, _1), kSubscribeRate); + gps.subscribe(std::bind( + &TimProduct::callbackTimTM2, this, std::placeholders::_1), kSubscribeRate); ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); - if (enabled["rxm_sfrb"]) - gps.subscribe(boost::bind( - publish, _1, "rxmsfrb"), kSubscribeRate); + if (enabled["rxm_sfrb"]) { + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmsfrb"), kSubscribeRate); + } // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) - gps.subscribe(boost::bind( - publish, _1, "rxmraw"), kSubscribeRate); + if (enabled["rxm_raw"]) { + gps.subscribe(std::bind( + publish, std::placeholders::_1, "rxmraw"), kSubscribeRate); + } } void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { From 578b8e4357d7208b82b06f8fb1a179bcd487bb7e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 13:57:13 -0500 Subject: [PATCH 012/152] Remove most of boost from node.cpp/.hpp. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 16 +++++-------- ublox_gps/src/node.cpp | 35 +++++++++++++++++----------- 2 files changed, 27 insertions(+), 24 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index e5fcec40..1330152a 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -33,10 +33,6 @@ // STL #include #include -// Boost -#include -#include -#include // ROS includes #include #include @@ -95,9 +91,9 @@ constexpr static uint32_t kNavSvInfoSubscribeRate = 20; // ROS objects //! ROS diagnostic updater -boost::shared_ptr updater; +std::shared_ptr updater; //! Node Handle for GPS node -boost::shared_ptr nh; +std::shared_ptr nh; //! Handles communication with the U-Blox Device ublox_gps::Gps gps; @@ -221,7 +217,7 @@ struct FixDiagnostic { }; //! fix frequency diagnostic updater -boost::shared_ptr freq_diag; +std::shared_ptr freq_diag; /** * @brief Determine dynamic model from human-readable string. @@ -468,7 +464,7 @@ class ComponentInterface { virtual void subscribe() = 0; }; -typedef boost::shared_ptr ComponentPtr; +typedef std::shared_ptr ComponentPtr; /** * @brief This class represents u-blox ROS node for *all* firmware and product @@ -595,7 +591,7 @@ class UbloxNode : public virtual ComponentInterface { * The node will call the functions in these interfaces for each object * in the vector. */ - std::vector > components_; + std::vector > components_; //! Determined From Mon VER float protocol_version_ = 0; @@ -1050,7 +1046,7 @@ class RawDataProduct: public virtual ComponentInterface { private: //! Topic diagnostic updaters - std::vector > freq_diagnostics_; + std::vector > freq_diagnostics_; }; /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 78eeb3c7..02369aa2 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -29,9 +29,12 @@ #include #include +#include #include #include +#include + #include "ublox_gps/node.hpp" using namespace ublox_node; @@ -384,31 +387,35 @@ void UbloxNode::processMonVer() { if(protocol_version_ < 18) { // Final line contains supported GNSS delimited by ; std::vector strs; - if(extension.size() > 0) + if (extension.size() > 0) { boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + } + for (size_t i = 0; i < strs.size(); i++) { supported.insert(strs[i]); + } } else { for(std::size_t i = 0; i < extension.size(); ++i) { std::vector strs; // Up to 2nd to last line - if(i <= extension.size() - 2) { + if (i <= extension.size() - 2) { boost::split(strs, extension[i], boost::is_any_of("=")); if(strs.size() > 1) { if (strs[0].compare(std::string("FWVER")) == 0) { - if(strs[1].length() > 8) + if (strs[1].length() > 8) { addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); - else + } else { addProductInterface(strs[1].substr(0, 3)); + } continue; } } } // Last 1-2 lines contain supported GNSS - if(i >= extension.size() - 2) { + if (i >= extension.size() - 2) { boost::split(strs, extension[i], boost::is_any_of(";")); - for(size_t i = 0; i < strs.size(); i++) + for (size_t i = 0; i < strs.size(); i++) { supported.insert(strs[i]); + } } } } @@ -523,9 +530,9 @@ void UbloxNode::configureInf() { void UbloxNode::initializeIo() { gps.setConfigOnStartup(config_on_startup_flag_); - boost::smatch match; - if (boost::regex_match(device_, match, - boost::regex("(tcp|udp)://(.+):(\\d+)"))) { + std::smatch match; + if (std::regex_match(device_, match, + std::regex("(tcp|udp)://(.+):(\\d+)"))) { std::string proto(match[1]); if (proto == "tcp") { std::string host(match[2]); @@ -1270,16 +1277,16 @@ void RawDataProduct::subscribe() { void RawDataProduct::initializeRosDiagnostics() { if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(boost::shared_ptr( + freq_diagnostics_.push_back(std::shared_ptr( new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(boost::shared_ptr( + freq_diagnostics_.push_back(std::shared_ptr( new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(boost::shared_ptr( + freq_diagnostics_.push_back(std::shared_ptr( new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(boost::shared_ptr( + freq_diagnostics_.push_back(std::shared_ptr( new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); } From 2403f39f9430b599333637c03bbb7a750b9d3b22 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 14:05:00 -0500 Subject: [PATCH 013/152] Remove the last of boost from node.cpp Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 104 ++++++++++++++++++++++++++++------------- 1 file changed, 72 insertions(+), 32 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 02369aa2..d32cedc5 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -32,8 +32,7 @@ #include #include #include - -#include +#include #include "ublox_gps/node.hpp" @@ -84,6 +83,27 @@ uint8_t ublox_node::fixModeFromString(const std::string& mode) { " is not a valid fix mode."); } +std::vector stringSplit(const std::string &str, + const std::string &splitter) { + std::vector ret; + size_t next = 0; + size_t current = next; + + if (splitter.empty()) { + // If the splitter is blank, just return the original + ret.push_back(str); + return ret; + } + + while (next != std::string::npos) { + next = str.find(splitter, current); + ret.push_back(str.substr(current, next - current)); + current = next + splitter.length(); + } + + return ret; +} + // // u-blox ROS Node // @@ -333,13 +353,15 @@ void UbloxNode::subscribe() { gps.subscribe(std::bind( publish, std::placeholders::_1, "aidhui"), kSubscribeRate); - for(int i = 0; i < components_.size(); i++) + for (size_t i = 0; i < components_.size(); i++) { components_[i]->subscribe(); + } } void UbloxNode::initializeRosDiagnostics() { - if (!nh->hasParam("diagnostic_period")) + if (!nh->hasParam("diagnostic_period")) { nh->setParam("diagnostic_period", kDiagnosticPeriod); + } updater.reset(new diagnostic_updater::Updater()); updater->setHardwareID("ublox"); @@ -347,22 +369,24 @@ void UbloxNode::initializeRosDiagnostics() { // configure diagnostic updater for frequency freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, kFixFreqWindow, kTimeStampStatusMin)); - for(int i = 0; i < components_.size(); i++) + for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); + } } void UbloxNode::processMonVer() { ublox_msgs::MonVER monVer; - if (!gps.poll(monVer)) + if (!gps.poll(monVer)) { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); + } ROS_DEBUG("%s, HW VER: %s", monVer.sw_version.c_array(), monVer.hw_version.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); - for(std::size_t i = 0; i < monVer.extension.size(); ++i) { + for (std::size_t i = 0; i < monVer.extension.size(); ++i) { ROS_DEBUG("%s", monVer.extension[i].field.c_array()); // Find the end of the string (null character) unsigned char* end = std::find(monVer.extension[i].field.begin(), @@ -371,7 +395,7 @@ void UbloxNode::processMonVer() { } // Get the protocol version - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) { std::size_t found = extension[i].find("PROTVER"); if (found != std::string::npos) { protocol_version_ = ::atof( @@ -379,16 +403,17 @@ void UbloxNode::processMonVer() { break; } } - if (protocol_version_ == 0) + if (protocol_version_ == 0) { ROS_WARN("Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); + } addFirmwareInterface(); - if(protocol_version_ < 18) { + if (protocol_version_ < 18) { // Final line contains supported GNSS delimited by ; std::vector strs; if (extension.size() > 0) { - boost::split(strs, extension[extension.size()-1], boost::is_any_of(";")); + strs = stringSplit(extension[extension.size() - 1], ";"); } for (size_t i = 0; i < strs.size(); i++) { supported.insert(strs[i]); @@ -398,8 +423,8 @@ void UbloxNode::processMonVer() { std::vector strs; // Up to 2nd to last line if (i <= extension.size() - 2) { - boost::split(strs, extension[i], boost::is_any_of("=")); - if(strs.size() > 1) { + strs = stringSplit(extension[i], "="); + if (strs.size() > 1) { if (strs[0].compare(std::string("FWVER")) == 0) { if (strs[1].length() > 8) { addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); @@ -412,7 +437,7 @@ void UbloxNode::processMonVer() { } // Last 1-2 lines contain supported GNSS if (i >= extension.size() - 2) { - boost::split(strs, extension[i], boost::is_any_of(";")); + strs = stringSplit(extension[i], ";"); for (size_t i = 0; i < strs.size(); i++) { supported.insert(strs[i]); } @@ -452,39 +477,45 @@ bool UbloxNode::configureUblox() { throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) { if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if (!gps.setPpp(enable_ppp_)) + if (!gps.setPpp(enable_ppp_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_ppp_) ? "enable" : "disable") + " PPP."); - if (!gps.setDynamicModel(dmodel_)) + } + if (!gps.setDynamicModel(dmodel_)) { throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); - if (!gps.setFixMode(fmode_)) + } + if (!gps.setFixMode(fmode_)) { throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); + } if (!gps.setDeadReckonLimit(dr_limit_)) { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (set_dat_ && !gps.configure(cfg_dat_)) + if (set_dat_ && !gps.configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); + } // Configure each component for (int i = 0; i < components_.size(); i++) { - if(!components_[i]->configureUblox()) + if (!components_[i]->configureUblox()) { return false; + } } } if (save_.save_mask != 0) { ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); - if(!gps.configure(save_)) + if (!gps.configure(save_)) { ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); + } } } catch (std::exception& e) { ROS_FATAL("Error configuring u-blox: %s", e.what()); @@ -504,7 +535,7 @@ void UbloxNode::configureInf() { (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); - for (int i = 0; i < block.inf_msg_mask.size(); i++) { + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } @@ -515,7 +546,7 @@ void UbloxNode::configureInf() { ublox_msgs::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port - for (int i = 0; i < block.inf_msg_mask.size(); i++) { + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } msg.blocks.push_back(block); @@ -562,12 +593,14 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if(protocol_version_ <= 14) { - if(nh->param("raw_data", false)) + if (nh->param("raw_data", false)) { components_.push_back(ComponentPtr(new RawDataProduct)); + } } // Must set firmware & hardware params before initializing diagnostics - for (int i = 0; i < components_.size(); i++) + for (size_t i = 0; i < components_.size(); i++) { components_[i]->getRosParams(); + } // Do this last initializeRosDiagnostics(); @@ -616,18 +649,22 @@ void UbloxFirmware6::getRosParams() { if (set_nmea_) { bool compat, consider; - if (!getRosUint("nmea/version", cfg_nmea_.version)) + if (!getRosUint("nmea/version", cfg_nmea_.version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) + } + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); - if (!nh->getParam("nmea/compat", compat)) + } + if (!nh->getParam("nmea/compat", compat)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) + } + if (!nh->getParam("nmea/consider", consider)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/consider must be set"); + } // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -653,8 +690,9 @@ void UbloxFirmware6::getRosParams() { bool UbloxFirmware6::configureUblox() { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - if (set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); + } return true; } @@ -678,16 +716,18 @@ void UbloxFirmware6::subscribe() { // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); - if (enabled["nav_svinfo"]) + if (enabled["nav_svinfo"]) { gps.subscribe(std::bind( publish, std::placeholders::_1, "navsvinfo"), kNavSvInfoSubscribeRate); + } // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) + if (enabled["mon_hw"]) { gps.subscribe(std::bind( publish, std::placeholders::_1, "monhw"), kSubscribeRate); + } } void UbloxFirmware6::fixDiagnostic( From f6044910f0f3af1b123884adf29aac9f94b422a9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 14:25:17 -0500 Subject: [PATCH 014/152] Remove uses of 'new' throughout the codebase. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 4 +- ublox_gps/include/ublox_gps/callback.hpp | 10 ++- ublox_gps/include/ublox_gps/node.hpp | 37 +++++------ ublox_gps/src/gps.cpp | 29 +++------ ublox_gps/src/node.cpp | 64 +++++++++++--------- 5 files changed, 70 insertions(+), 74 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 47aae1a8..2e69e9ee 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -147,8 +147,8 @@ AsyncWorker::AsyncWorker(std::shared_ptr stream, out_.reserve(buffer_size); io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_.reset(new std::thread( - boost::bind(&boost::asio::io_service::run, io_service_))); + background_thread_ = std::make_shared( + boost::bind(&boost::asio::io_service::run, io_service_)); } template diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 264fc6d3..f0ef963e 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -132,10 +132,9 @@ class CallbackHandlers { template void insert(typename CallbackHandler_::Callback callback) { std::lock_guard lock(callback_mutex_); - CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - std::shared_ptr(handler))); + std::make_shared>(callback))); } /** @@ -151,10 +150,9 @@ class CallbackHandlers { typename CallbackHandler_::Callback callback, unsigned int message_id) { std::lock_guard lock(callback_mutex_); - CallbackHandler_* handler = new CallbackHandler_(callback); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, message_id), - std::shared_ptr(handler))); + std::make_shared>(callback))); } /** @@ -182,10 +180,10 @@ class CallbackHandlers { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); - CallbackHandler_* handler = new CallbackHandler_(); + auto handler = std::make_shared>(); Callbacks::iterator callback = callbacks_.insert( (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - std::shared_ptr(handler)))); + handler))); callback_mutex_.unlock(); // Wait for the message diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 1330152a..0645c3af 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -31,8 +31,9 @@ #define UBLOX_GPS_NODE_HPP // STL -#include +#include #include +#include // ROS includes #include #include @@ -143,9 +144,9 @@ struct UbloxTopicDiagnostic { max_freq = target_freq; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic = std::make_shared(topic, + *updater, + freq_param); } /** @@ -164,13 +165,13 @@ struct UbloxTopicDiagnostic { max_freq = freq_max; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, freq_tol, freq_window); - diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic, - *updater, - freq_param); + diagnostic = std::make_shared(topic, + *updater, + freq_param); } //! Topic frequency diagnostic updater - diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic; + std::shared_ptr diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -202,14 +203,14 @@ struct FixDiagnostic { freq_tol, freq_window); double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = new diagnostic_updater::TopicDiagnostic(name, - *updater, - freq_param, - time_param); + diagnostic = std::make_shared(name, + *updater, + freq_param, + time_param); } //! Topic frequency diagnostic updater - diagnostic_updater::TopicDiagnostic *diagnostic; + std::shared_ptr diagnostic; //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic @@ -491,11 +492,11 @@ class UbloxNode : public virtual ComponentInterface { //! [s] 5Hz diagnostic period constexpr static float kDiagnosticPeriod = 0.2; //! Tolerance for Fix topic frequency as percentage of target frequency - constexpr static double kFixFreqTol = 0.15; + double kFixFreqTol = 0.15; //! Window [num messages] for Fix Frequency Diagnostic - constexpr static double kFixFreqWindow = 10; + double kFixFreqWindow = 10; //! Minimum Time Stamp Status for fix frequency diagnostic - constexpr static double kTimeStampStatusMin = 0; + double kTimeStampStatusMin = 0; /** * @brief Initialize and run the u-blox node. @@ -1018,8 +1019,8 @@ class UbloxFirmware9 : public UbloxFirmware8 { */ class RawDataProduct: public virtual ComponentInterface { public: - static constexpr double kRtcmFreqTol = 0.15; - static constexpr int kRtcmFreqWindow = 25; + double kRtcmFreqTol = 0.15; + int kRtcmFreqWindow = 25; /** * @brief Does nothing since there are no Raw Data product specific settings. diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 5df4bc96..318fd0f7 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -113,10 +113,8 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { void Gps::initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) { port_ = port; - std::shared_ptr io_service( - new boost::asio::io_service); - std::shared_ptr serial( - new boost::asio::serial_port(*io_service)); + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { @@ -143,8 +141,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, if (worker_) { return; } - setWorker(std::shared_ptr( - new AsyncWorker(serial, io_service))); + setWorker(std::make_shared>(serial, io_service)); configured_ = false; @@ -178,10 +175,8 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } void Gps::resetSerial(std::string port) { - std::shared_ptr io_service( - new boost::asio::io_service); - std::shared_ptr serial( - new boost::asio::serial_port(*io_service)); + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { @@ -197,8 +192,7 @@ void Gps::resetSerial(std::string port) { if (worker_) { return; } - setWorker(std::shared_ptr( - new AsyncWorker(serial, io_service))); + setWorker(std::make_shared>(serial, io_service)); configured_ = false; // Poll UART PRT Config @@ -223,8 +217,7 @@ void Gps::resetSerial(std::string port) { void Gps::initializeTcp(std::string host, std::string port) { host_ = host; port_ = port; - std::shared_ptr io_service( - new boost::asio::io_service); + auto io_service = std::make_shared(); boost::asio::ip::tcp::resolver::iterator endpoint; try { @@ -236,8 +229,7 @@ void Gps::initializeTcp(std::string host, std::string port) { port + " " + e.what()); } - std::shared_ptr socket( - new boost::asio::ip::tcp::socket(*io_service)); + auto socket = std::make_shared(*io_service); try { socket->connect(*endpoint); @@ -253,9 +245,8 @@ void Gps::initializeTcp(std::string host, std::string port) { if (worker_) { return; } - setWorker(std::shared_ptr( - new AsyncWorker(socket, - io_service))); + setWorker(std::make_shared>(socket, + io_service)); } void Gps::close() { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d32cedc5..e2cdea0e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -114,16 +114,16 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(ComponentPtr(new UbloxFirmware6)); + components_.push_back(std::make_shared()); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(ComponentPtr(new UbloxFirmware7)); + components_.push_back(std::make_shared()); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(ComponentPtr(new UbloxFirmware8)); + components_.push_back(std::make_shared()); ublox_version = 8; } else { - components_.push_back(ComponentPtr(new UbloxFirmware9)); + components_.push_back(std::make_shared()); ublox_version = 9; } @@ -134,18 +134,18 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(std::string product_category, std::string ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) - components_.push_back(ComponentPtr(new HpgRefProduct)); + components_.push_back(std::make_shared()); else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) - components_.push_back(ComponentPtr(new HpgRovProduct)); + components_.push_back(std::make_shared()); else if (product_category.compare("HPG") == 0) - components_.push_back(ComponentPtr(new HpPosRecProduct)); + components_.push_back(std::make_shared()); else if (product_category.compare("TIM") == 0) - components_.push_back(ComponentPtr(new TimProduct)); + components_.push_back(std::make_shared()); else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) - components_.push_back(ComponentPtr(new AdrUdrProduct)); + components_.push_back(std::make_shared()); else if (product_category.compare("FTS") == 0) - components_.push_back(ComponentPtr(new FtsProduct)); + components_.push_back(std::make_shared()); else if(product_category.compare("SPG") != 0) ROS_WARN("Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), @@ -363,12 +363,12 @@ void UbloxNode::initializeRosDiagnostics() { nh->setParam("diagnostic_period", kDiagnosticPeriod); } - updater.reset(new diagnostic_updater::Updater()); + updater = std::make_shared(); updater->setHardwareID("ublox"); // configure diagnostic updater for frequency - freq_diag.reset(new FixDiagnostic(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin)); + freq_diag = std::make_shared(std::string("fix"), kFixFreqTol, + kFixFreqWindow, kTimeStampStatusMin); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } @@ -594,7 +594,7 @@ void UbloxNode::initialize() { processMonVer(); if(protocol_version_ <= 14) { if (nh->param("raw_data", false)) { - components_.push_back(ComponentPtr(new RawDataProduct)); + components_.push_back(std::make_shared()); } } // Must set firmware & hardware params before initializing diagnostics @@ -1316,18 +1316,22 @@ void RawDataProduct::subscribe() { } void RawDataProduct::initializeRosDiagnostics() { - if (enabled["rxm_raw"]) - freq_diagnostics_.push_back(std::shared_ptr( - new UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_sfrb"]) - freq_diagnostics_.push_back(std::shared_ptr( - new UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_eph"]) - freq_diagnostics_.push_back(std::shared_ptr( - new UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol, kRtcmFreqWindow))); - if (enabled["rxm_alm"]) - freq_diagnostics_.push_back(std::shared_ptr( - new UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol, kRtcmFreqWindow))); + if (enabled["rxm_raw"]) { + freq_diagnostics_.push_back(std::make_shared( + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow)); + } + if (enabled["rxm_sfrb"]) { + freq_diagnostics_.push_back(std::make_shared( + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow)); + } + if (enabled["rxm_eph"]) { + freq_diagnostics_.push_back(std::make_shared( + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow)); + } + if (enabled["rxm_alm"]) { + freq_diagnostics_.push_back(std::make_shared( + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow)); + } } // @@ -1337,14 +1341,16 @@ void AdrUdrProduct::getRosParams() { nh->param("use_adr", use_adr_, true); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate * nav_rate); - if(nav_rate_hz != 1) + if (nav_rate_hz != 1) { ROS_WARN("Nav Rate recommended to be 1 Hz"); + } } bool AdrUdrProduct::configureUblox() { - if(!gps.setUseAdr(use_adr_)) + if (!gps.setUseAdr(use_adr_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); + } return true; } @@ -1892,7 +1898,7 @@ void TimProduct::initializeRosDiagnostics() { int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); - nh.reset(new ros::NodeHandle("~")); + nh = std::make_shared("~"); nh->param("debug", ublox_gps::debug, 1); if(ublox_gps::debug) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, From cd06a0f296899b459431cbb5f525f5ed1fcad976 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 14:49:37 -0500 Subject: [PATCH 015/152] Switch to non-boost asio. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 15 ++---- ublox_gps/cmake/Findasio.cmake | 5 ++ ublox_gps/include/ublox_gps/async_worker.hpp | 44 ++++++++--------- ublox_gps/include/ublox_gps/gps.hpp | 4 -- ublox_gps/src/gps.cpp | 50 ++++++++++---------- 5 files changed, 55 insertions(+), 63 deletions(-) create mode 100644 ublox_gps/cmake/Findasio.cmake diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index f3dc3fc1..d72b8a9f 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -14,15 +14,13 @@ catkin_package( LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS tf roscpp ublox_msgs ublox_serialization) -# include boost -find_package(Boost REQUIRED COMPONENTS system regex thread) -link_directories(${Boost_LIBRARY_DIR}) -include_directories(${Boost_INCLUDE_DIR}) - # include other ublox packages include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${catkin_INCLUDE_DIRS}) +set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +find_package(asio REQUIRED) + # link pthread SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread") @@ -32,12 +30,6 @@ add_library(ublox_gps src/gps.cpp) # fix msg compile order bug add_dependencies(ublox_gps ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ublox_gps - boost_system - boost_regex - boost_thread -) - target_link_libraries(ublox_gps ${catkin_LIBRARIES} ) @@ -46,7 +38,6 @@ target_link_libraries(ublox_gps add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) -target_link_libraries(ublox_gps_node boost_system boost_regex boost_thread) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) target_link_libraries(ublox_gps_node ublox_gps) diff --git a/ublox_gps/cmake/Findasio.cmake b/ublox_gps/cmake/Findasio.cmake new file mode 100644 index 00000000..002b599a --- /dev/null +++ b/ublox_gps/cmake/Findasio.cmake @@ -0,0 +1,5 @@ +include(FindPackageHandleStandardArgs) + +find_path(ASIO_INCLUDE_DIR NAMES asio.hpp) +find_package_handle_standard_args(asio DEFAULT_MSG ASIO_INCLUDE_DIR) +mark_as_advanced(ASIO_INCLUDE_DIR) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 2e69e9ee..5115d89e 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -31,15 +31,17 @@ #include #include +#include #include #include #include #include -#include - -#include -#include +#include +#include +#include +#include +#include #include "worker.hpp" @@ -60,7 +62,7 @@ class AsyncWorker : public Worker { * @param buffer_size the size of the input and output buffers */ AsyncWorker(std::shared_ptr stream, - std::shared_ptr io_service, + std::shared_ptr io_service, std::size_t buffer_size = 8192); virtual ~AsyncWorker(); @@ -101,7 +103,7 @@ class AsyncWorker : public Worker { * @param error_code an error code for read failures * @param the number of bytes received */ - void readEnd(const boost::system::error_code&, std::size_t); + void readEnd(const asio::error_code&, std::size_t); /** * @brief Send all the data in the output buffer. @@ -114,7 +116,7 @@ class AsyncWorker : public Worker { void doClose(); std::shared_ptr stream_; //!< The I/O stream - std::shared_ptr io_service_; //!< The I/O service + std::shared_ptr io_service_; //!< The I/O service std::mutex read_mutex_; //!< Lock for the input buffer std::condition_variable read_condition_; @@ -136,7 +138,7 @@ class AsyncWorker : public Worker { template AsyncWorker::AsyncWorker(std::shared_ptr stream, - std::shared_ptr io_service, + std::shared_ptr io_service, std::size_t buffer_size) : stopping_(false) { stream_ = stream; @@ -146,14 +148,13 @@ AsyncWorker::AsyncWorker(std::shared_ptr stream, out_.reserve(buffer_size); - io_service_->post(boost::bind(&AsyncWorker::doRead, this)); - background_thread_ = std::make_shared( - boost::bind(&boost::asio::io_service::run, io_service_)); + io_service_->post(std::bind(&AsyncWorker::doRead, this)); + background_thread_ = std::make_shared([this]{ io_service_->run(); }); } template AsyncWorker::~AsyncWorker() { - io_service_->post(boost::bind(&AsyncWorker::doClose, this)); + io_service_->post(std::bind(&AsyncWorker::doClose, this)); background_thread_->join(); //io_service_->reset(); } @@ -173,7 +174,7 @@ bool AsyncWorker::send(const unsigned char* data, } out_.insert(out_.end(), data, data + size); - io_service_->post(boost::bind(&AsyncWorker::doWrite, this)); + io_service_->post(std::bind(&AsyncWorker::doWrite, this)); return true; } @@ -185,7 +186,7 @@ void AsyncWorker::doWrite() { return; } // Write all the data in the out buffer - boost::asio::write(*stream_, boost::asio::buffer(out_.data(), out_.size())); + asio::write(*stream_, asio::buffer(out_.data(), out_.size())); if (debug >= 2) { // Print the data that was sent @@ -205,15 +206,14 @@ template void AsyncWorker::doRead() { std::lock_guard lock(read_mutex_); stream_->async_read_some( - boost::asio::buffer(in_.data() + in_buffer_size_, - in_.size() - in_buffer_size_), - boost::bind(&AsyncWorker::readEnd, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + asio::buffer(in_.data() + in_buffer_size_, + in_.size() - in_buffer_size_), + std::bind(&AsyncWorker::readEnd, this, + std::placeholders::_1, std::placeholders::_2)); } template -void AsyncWorker::readEnd(const boost::system::error_code& error, +void AsyncWorker::readEnd(const asio::error_code& error, std::size_t bytes_transfered) { std::lock_guard lock(read_mutex_); if (error) { @@ -249,7 +249,7 @@ void AsyncWorker::readEnd(const boost::system::error_code& error, } if (!stopping_) { - io_service_->post(boost::bind(&AsyncWorker::doRead, this)); + io_service_->post(std::bind(&AsyncWorker::doRead, this)); } } @@ -257,7 +257,7 @@ template void AsyncWorker::doClose() { std::lock_guard lock(read_mutex_); stopping_ = true; - boost::system::error_code error; + asio::error_code error; stream_->close(error); if (error) { ROS_ERROR_STREAM( diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 301ed4fb..b17b5c57 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -36,10 +36,6 @@ #include #include #include -// Boost -#include -#include -#include // ROS #include // Other u-blox packages diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 318fd0f7..bc4c6eaa 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -33,6 +33,11 @@ #include #include +#include +#include +#include +#include + #include namespace ublox_gps { @@ -113,8 +118,8 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { void Gps::initializeSerial(std::string port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) { port_ = port; - auto io_service = std::make_shared(); - auto serial = std::make_shared(*io_service); + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { @@ -126,27 +131,22 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); - if (BOOST_VERSION < 106600) { - // NOTE(Kartik): Set serial port to "raw" mode. This is done in Boost but - // until v1.66.0 there was a bug which didn't enable the relevant code, - // fixed by commit: https://github.com/boostorg/asio/commit/619cea4356 - int fd = serial->native_handle(); - termios tio; - tcgetattr(fd, &tio); - cfmakeraw(&tio); - tcsetattr(fd, TCSANOW, &tio); - } + int fd = serial->native_handle(); + termios tio; + tcgetattr(fd, &tio); + cfmakeraw(&tio); + tcsetattr(fd, TCSANOW, &tio); // Set the I/O worker if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service)); + setWorker(std::make_shared>(serial, io_service)); configured_ = false; // Set the baudrate - boost::asio::serial_port_base::baud_rate current_baudrate; + asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { @@ -158,7 +158,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, continue; } serial->set_option( - boost::asio::serial_port_base::baud_rate(kBaudrates[i])); + asio::serial_port_base::baud_rate(kBaudrates[i])); std::this_thread::sleep_for( std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); @@ -175,8 +175,8 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } void Gps::resetSerial(std::string port) { - auto io_service = std::make_shared(); - auto serial = std::make_shared(*io_service); + auto io_service = std::make_shared(); + auto serial = std::make_shared(*io_service); // open serial port try { @@ -192,7 +192,7 @@ void Gps::resetSerial(std::string port) { if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service)); + setWorker(std::make_shared>(serial, io_service)); configured_ = false; // Poll UART PRT Config @@ -210,26 +210,26 @@ void Gps::resetSerial(std::string port) { } // Set the baudrate - serial->set_option(boost::asio::serial_port_base::baud_rate(prt.baud_rate)); + serial->set_option(asio::serial_port_base::baud_rate(prt.baud_rate)); configured_ = true; } void Gps::initializeTcp(std::string host, std::string port) { host_ = host; port_ = port; - auto io_service = std::make_shared(); - boost::asio::ip::tcp::resolver::iterator endpoint; + auto io_service = std::make_shared(); + asio::ip::tcp::resolver::iterator endpoint; try { - boost::asio::ip::tcp::resolver resolver(*io_service); + asio::ip::tcp::resolver resolver(*io_service); endpoint = - resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); + resolver.resolve(asio::ip::tcp::resolver::query(host, port)); } catch (std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } - auto socket = std::make_shared(*io_service); + auto socket = std::make_shared(*io_service); try { socket->connect(*endpoint); @@ -245,7 +245,7 @@ void Gps::initializeTcp(std::string host, std::string port) { if (worker_) { return; } - setWorker(std::make_shared>(socket, + setWorker(std::make_shared>(socket, io_service)); } From 77e5ba3c2672d60f39ecd2d0cb2bd5a453939e84 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 15:03:35 -0500 Subject: [PATCH 016/152] Remove 'using namespace' uses. Signed-off-by: Chris Lalancette --- ublox_gps/src/gps.cpp | 32 +++++++++++++++----------------- ublox_gps/src/logger_node_pa.cpp | 4 +--- ublox_gps/src/node.cpp | 22 ++++++++++++---------- ublox_gps/src/raw_data_pa.cpp | 5 +++-- 4 files changed, 31 insertions(+), 32 deletions(-) diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index bc4c6eaa..d79a25bf 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -42,8 +42,6 @@ namespace ublox_gps { -using namespace ublox_msgs; - const std::chrono::milliseconds Gps::default_timeout_ = std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); @@ -100,7 +98,7 @@ void Gps::processNack(const ublox_msgs::Ack &m) { } void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { - if (m.cmd == UpdSOSAck::CMD_BACKUP_CREATE_ACK) { + if (m.cmd == ublox_msgs::UpdSOSAck::CMD_BACKUP_CREATE_ACK) { Ack ack; ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; ack.class_id = m.CLASS_ID; @@ -197,8 +195,8 @@ void Gps::resetSerial(std::string port) { // Poll UART PRT Config std::vector payload; - payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + payload.push_back(ublox_msgs::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::CfgPRT::CLASS_ID, ublox_msgs::CfgPRT::MESSAGE_ID, payload)) { ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } @@ -288,7 +286,7 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { return true; } -bool Gps::configGnss(CfgGNSS gnss, +bool Gps::configGnss(ublox_msgs::CfgGNSS gnss, const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox ROS_DEBUG("Re-configuring GNSS."); @@ -297,7 +295,7 @@ bool Gps::configGnss(CfgGNSS gnss, } // Cold reset the GNSS ROS_WARN("GNSS re-configured, cold resetting device."); - if (!configReset(CfgRST::NAV_BBR_COLD_START, CfgRST::RESET_MODE_GNSS)) { + if (!configReset(ublox_msgs::CfgRST::NAV_BBR_COLD_START, ublox_msgs::CfgRST::RESET_MODE_GNSS)) { return false; } ros::Duration(1.0).sleep(); @@ -338,22 +336,22 @@ bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, baudrate, in_proto_mask, out_proto_mask); ublox_msgs::CfgPRT port; - port.port_id = CfgPRT::PORT_ID_UART1; + port.port_id = ublox_msgs::CfgPRT::PORT_ID_UART1; port.baud_rate = baudrate; - port.mode = CfgPRT::MODE_RESERVED1 | CfgPRT::MODE_CHAR_LEN_8BIT | - CfgPRT::MODE_PARITY_NO | CfgPRT::MODE_STOP_BITS_1; + port.mode = ublox_msgs::CfgPRT::MODE_RESERVED1 | ublox_msgs::CfgPRT::MODE_CHAR_LEN_8BIT | + ublox_msgs::CfgPRT::MODE_PARITY_NO | ublox_msgs::CfgPRT::MODE_STOP_BITS_1; port.in_proto_mask = in_proto_mask; port.out_proto_mask = out_proto_mask; return configure(port); } -bool Gps::disableUart1(CfgPRT& prev_config) { +bool Gps::disableUart1(ublox_msgs::CfgPRT& prev_config) { ROS_DEBUG("Disabling UART1"); // Poll UART PRT Config std::vector payload; - payload.push_back(CfgPRT::PORT_ID_UART1); - if (!poll(CfgPRT::CLASS_ID, CfgPRT::MESSAGE_ID, payload)) { + payload.push_back(ublox_msgs::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::CfgPRT::CLASS_ID, ublox_msgs::CfgPRT::MESSAGE_ID, payload)) { ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } @@ -363,7 +361,7 @@ bool Gps::disableUart1(CfgPRT& prev_config) { } // Keep original settings, but disable in/out ublox_msgs::CfgPRT port; - port.port_id = CfgPRT::PORT_ID_UART1; + port.port_id = ublox_msgs::CfgPRT::PORT_ID_UART1; port.mode = prev_config.mode; port.baud_rate = prev_config.baud_rate; port.in_proto_mask = 0; @@ -384,7 +382,7 @@ bool Gps::configUsb(uint16_t tx_ready, tx_ready, in_proto_mask, out_proto_mask); ublox_msgs::CfgPRT port; - port.port_id = CfgPRT::PORT_ID_USB; + port.port_id = ublox_msgs::CfgPRT::PORT_ID_USB; port.tx_ready = tx_ready; port.in_proto_mask = in_proto_mask; port.out_proto_mask = out_proto_mask; @@ -398,7 +396,7 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { ublox_msgs::CfgRATE rate; rate.meas_rate = meas_rate; rate.nav_rate = nav_rate; // must be fixed at 1 for ublox 5 and 6 - rate.time_ref = CfgRATE::TIME_REF_GPS; + rate.time_ref = ublox_msgs::CfgRATE::TIME_REF_GPS; return configure(rate); } @@ -417,7 +415,7 @@ bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); ublox_msgs::CfgSBAS msg; - msg.mode = (enable ? CfgSBAS::MODE_ENABLED : 0); + msg.mode = (enable ? ublox_msgs::CfgSBAS::MODE_ENABLED : 0); msg.usage = usage; msg.max_sbas = max_sbas; return configure(msg); diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index 6c05917e..d18c2031 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -37,8 +37,6 @@ // Ublox GPS includes #include -using namespace ublox_node; - // // Raw Data Stream (feature from TUC-ProAut) // @@ -47,7 +45,7 @@ int main(int argc, char** argv) { ros::init(argc, argv, "ublox_logger"); - RawDataStreamPa node(true); + ublox_node::RawDataStreamPa node(true); node.getRosParams(); node.initialize(); diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index e2cdea0e..4934cca4 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -36,12 +36,12 @@ #include "ublox_gps/node.hpp" -using namespace ublox_node; +namespace ublox_node { // // ublox_node namespace // -uint8_t ublox_node::modelFromString(const std::string& model) { +uint8_t modelFromString(const std::string& model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if(lower == "portable") { @@ -68,7 +68,7 @@ uint8_t ublox_node::modelFromString(const std::string& model) { " is not a valid dynamic model."); } -uint8_t ublox_node::fixModeFromString(const std::string& mode) { +uint8_t fixModeFromString(const std::string& mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { @@ -1896,16 +1896,18 @@ void TimProduct::initializeRosDiagnostics() { updater->force_update(); } +} // namespace ublox_node + int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); - nh = std::make_shared("~"); - nh->param("debug", ublox_gps::debug, 1); - if(ublox_gps::debug) { + ublox_node::nh = std::make_shared("~"); + ublox_node::nh->param("debug", ublox_gps::debug, 1); + if (ublox_gps::debug) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - + ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } } - UbloxNode node; + ublox_node::UbloxNode node; return 0; } diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 1d440977..4093fe47 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -41,12 +41,12 @@ #include #include -using namespace ublox_node; - // // ublox_node namespace // +namespace ublox_node { + RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : pnh_(ros::NodeHandle("~")), flag_publish_(false), @@ -190,3 +190,4 @@ void RawDataStreamPa::saveToFile(const std::string str) { } } +} // namespace ublox_node From 69d096aa7d3fd49d7bac3f4ebe0fb72f9078897d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 15:21:16 -0500 Subject: [PATCH 017/152] Switch the one use of tf to tf2. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 4 ++-- ublox_gps/include/ublox_gps/node.hpp | 1 - ublox_gps/package.xml | 2 +- ublox_gps/src/node.cpp | 4 +++- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index d72b8a9f..98b1b258 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(ublox_gps) find_package(catkin REQUIRED COMPONENTS - tf roscpp roscpp_serialization + tf2 ublox_msgs ublox_serialization diagnostic_updater @@ -12,7 +12,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS tf roscpp ublox_msgs ublox_serialization) + CATKIN_DEPENDS tf2 roscpp ublox_msgs ublox_serialization) # include other ublox packages include_directories(${PROJECT_SOURCE_DIR}/include) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 0645c3af..fcc35f50 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include // ROS messages diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 59e28bc8..afde427e 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -18,7 +18,7 @@ ublox_msgs roscpp roscpp_serialization - tf + tf2 diagnostic_updater diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 4934cca4..35998f90 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -34,6 +34,8 @@ #include #include +#include + #include "ublox_gps/node.hpp" namespace ublox_node { @@ -1799,7 +1801,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { imu_.angular_velocity_covariance[0] = -1; double heading = static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI; - tf::Quaternion orientation; + tf2::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; imu_.orientation.y = orientation[1]; From e77aba6ee4645efcb12cba125f409f5da745f09d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 16:25:21 -0500 Subject: [PATCH 018/152] Switch to more idiomatic publisher initialization. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 60 ++++-- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 160 +++++++------- ublox_gps/src/node.cpp | 226 ++++++++++++-------- ublox_gps/src/raw_data_pa.cpp | 180 ++++++++-------- 4 files changed, 358 insertions(+), 268 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index fcc35f50..c73c7199 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -739,6 +739,12 @@ class UbloxFirmware6 : public UbloxFirmware { ublox_msgs::CfgNMEA6 cfg_nmea_; //! Whether or not to configure the NMEA settings bool set_nmea_; + + ros::Publisher nav_pos_llh_pub_; + ros::Publisher fix_pub_; + ros::Publisher nav_vel_ned_pub_; + ros::Publisher vel_pub_; + ros::Publisher nav_sol_pub_; }; /** @@ -753,6 +759,17 @@ class UbloxFirmware6 : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: + UbloxFirmware7Plus() { + // NavPVT publisher + nav_pvt_pub_ = nh->advertise("navpvt", kROSQueueSize); + + fix_pub_ = + nh->advertise("fix", kROSQueueSize); + vel_pub_ = + nh->advertise("fix_velocity", + kROSQueueSize); + } + /** * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. * @@ -764,17 +781,12 @@ class UbloxFirmware7Plus : public UbloxFirmware { void callbackNavPvt(const NavPVT& m) { if (enabled["nav_pvt"]) { // NavPVT publisher - static ros::Publisher publisher = nh->advertise("navpvt", - kROSQueueSize); - publisher.publish(m); + nav_pvt_pub_.publish(m); } // // NavSatFix message // - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); - sensor_msgs::NavSatFix fix; fix.header.frame_id = frame_id; // set the timestamp @@ -823,14 +835,11 @@ class UbloxFirmware7Plus : public UbloxFirmware { fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - fixPublisher.publish(fix); + fix_pub_.publish(fix); // // Twist message // - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); geometry_msgs::TwistWithCovarianceStamped velocity; velocity.header.stamp = fix.header.stamp; velocity.header.frame_id = frame_id; @@ -847,7 +856,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { velocity.twist.covariance[cols * 2 + 2] = cov_speed; velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - velocityPublisher.publish(velocity); + vel_pub_.publish(velocity); // // Update diagnostics @@ -919,6 +928,10 @@ class UbloxFirmware7Plus : public UbloxFirmware { bool enable_sbas_; //! The QZSS Signal configuration, see CfgGNSS message uint32_t qzss_sig_cfg_; + + ros::Publisher nav_pvt_pub_; + ros::Publisher fix_pub_; + ros::Publisher vel_pub_; }; /** @@ -1055,6 +1068,8 @@ class RawDataProduct: public virtual ComponentInterface { */ class AdrUdrProduct: public virtual ComponentInterface { public: + AdrUdrProduct(); + /** * @brief Get the ADR/UDR parameters. * @@ -1090,12 +1105,14 @@ class AdrUdrProduct: public virtual ComponentInterface { //! Whether or not to enable dead reckoning bool use_adr_; - sensor_msgs::Imu imu_; sensor_msgs::TimeReference t_ref_; ublox_msgs::TimTM2 timtm2; void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); + + ros::Publisher imu_pub_; + ros::Publisher time_ref_pub_; }; /** @@ -1137,6 +1154,7 @@ class FtsProduct: public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: + HpgRefProduct(); /** * @brief Get the ROS parameters specific to the Reference Station * configuration. @@ -1239,6 +1257,8 @@ class HpgRefProduct: public virtual ComponentInterface { SURVEY_IN, //!< Survey-In mode TIME //!< Time mode, after survey-in or after configuring fixed mode } mode_; + + ros::Publisher navsvin_pub_; }; /** @@ -1255,6 +1275,9 @@ class HpgRovProduct: public virtual ComponentInterface { constexpr static double kRtcmFreqTol = 0.1; //! Diagnostic updater: RTCM topic frequency window [num messages] constexpr static int kRtcmFreqWindow = 25; + + HpgRovProduct(); + /** * @brief Get the ROS parameters specific to the Rover configuration. * @@ -1306,10 +1329,14 @@ class HpgRovProduct: public virtual ComponentInterface { //! The RTCM topic frequency diagnostic updater UbloxTopicDiagnostic freq_rtcm_; + + ros::Publisher nav_rel_pos_ned_pub_; }; class HpPosRecProduct: public virtual HpgRefProduct { public: + HpPosRecProduct(); + /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ @@ -1328,6 +1355,9 @@ class HpPosRecProduct: public virtual HpgRefProduct { //! Last relative position (used for diagnostic updater) ublox_msgs::NavRELPOSNED9 last_rel_pos_; + + ros::Publisher nav_relposned_pub_; + ros::Publisher imu_pub_; }; /** @@ -1335,6 +1365,9 @@ class HpPosRecProduct: public virtual HpgRefProduct { * @todo partially implemented */ class TimProduct: public virtual ComponentInterface { + public: + TimProduct(); + /** * @brief Get the Time Sync parameters. * @todo Currently unimplemented. @@ -1368,6 +1401,9 @@ class TimProduct: public virtual ComponentInterface { void callbackTimTM2(const ublox_msgs::TimTM2 &m); sensor_msgs::TimeReference t_ref_; + + ros::Publisher timtm2_pub_; + ros::Publisher interrupt_time_pub_; }; } diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 95d71ac9..2fd22c37 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -37,9 +37,9 @@ #define UBLOX_RAW_DATA_PA_H // STL -#include -#include #include +#include +#include // ROS includes #include @@ -58,85 +58,87 @@ namespace ublox_node { * @brief Implements functions for raw data stream. */ class RawDataStreamPa { - public: - - - /** - * @brief Constructor. - * Initialises variables and the nodehandle. - */ - RawDataStreamPa(bool is_ros_subscriber = false); - - /** - * @brief Get the raw data stream parameters. - */ - void getRosParams(void); - - /** - * @brief Returns the if raw data streaming is enabled. - */ - bool isEnabled(void); - - /** - * @brief Initializes raw data streams - * If storing to file is enabled, the filename is created and the - * corresponding filedescriptor will be opened. - * If publishing ros messages is enabled, an empty msg will be published. - * (This will implicitly create the publisher) - */ - void initialize(void); - - /** - * @brief Callback function which handles raw data. - * @param data the buffer of u-blox messages to process - * @param size the size of the buffer - */ - void ubloxCallback(const unsigned char* data, + public: + + + /** + * @brief Constructor. + * Initialises variables and the nodehandle. + */ + RawDataStreamPa(bool is_ros_subscriber = false); + + /** + * @brief Get the raw data stream parameters. + */ + void getRosParams(void); + + /** + * @brief Returns the if raw data streaming is enabled. + */ + bool isEnabled(void); + + /** + * @brief Initializes raw data streams + * If storing to file is enabled, the filename is created and the + * corresponding filedescriptor will be opened. + * If publishing ros messages is enabled, an empty msg will be published. + * (This will implicitly create the publisher) + */ + void initialize(void); + + /** + * @brief Callback function which handles raw data. + * @param data the buffer of u-blox messages to process + * @param size the size of the buffer + */ + void ubloxCallback(const unsigned char* data, const std::size_t size); - /** - * @brief Callback function which handles raw data. - * @param msg ros message - */ - void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); - - private: - /** - * @brief Converts a string into an uint8 multibyte array - */ - std_msgs::UInt8MultiArray str2uint8(const std::string str); - - /** - * @brief Publishes data stream as ros message - * @param str raw data stream as string - */ - void publishMsg(const std::string str); - - /** - * @brief Stores data to given file - * @param str raw data stream as string - */ - void saveToFile(const std::string str); - - //! Directoy name for storing raw data - std::string file_dir_; - //! Filename for storing raw data - std::string file_name_; - //! Handle for file access - std::ofstream file_handle_; - - //! Flag for publishing raw data - bool flag_publish_; - - //! Internal flag - //! true : subscribing to ros messages and storing those to file - //! false: publishing ros messages and/or storing to file - bool is_ros_subscriber_; - - //! ROS private node handle (for params and publisher) - ros::NodeHandle pnh_; - //! ROS node handle (only for subscriber) - ros::NodeHandle nh_; + /** + * @brief Callback function which handles raw data. + * @param msg ros message + */ + void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); + + private: + /** + * @brief Converts a string into an uint8 multibyte array + */ + std_msgs::UInt8MultiArray str2uint8(const std::string str); + + /** + * @brief Publishes data stream as ros message + * @param str raw data stream as string + */ + void publishMsg(const std::string str); + + /** + * @brief Stores data to given file + * @param str raw data stream as string + */ + void saveToFile(const std::string str); + + //! Directoy name for storing raw data + std::string file_dir_; + //! Filename for storing raw data + std::string file_name_; + //! Handle for file access + std::ofstream file_handle_; + + //! Flag for publishing raw data + bool flag_publish_; + + //! Internal flag + //! true : subscribing to ros messages and storing those to file + //! false: publishing ros messages and/or storing to file + bool is_ros_subscriber_; + + //! ROS private node handle (for params and publisher) + ros::NodeHandle pnh_; + //! ROS node handle (only for subscriber) + ros::NodeHandle nh_; + + ros::Publisher raw_pub_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 35998f90..231b27dd 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -641,7 +642,23 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6() {} +UbloxFirmware6::UbloxFirmware6() +{ + nav_pos_llh_pub_ = + nh->advertise("navposllh", kROSQueueSize); + fix_pub_ = + nh->advertise("fix", kROSQueueSize); + + nav_vel_ned_pub_ = + nh->advertise("navvelned", kROSQueueSize); + + vel_pub_ = + nh->advertise("fix_velocity", + kROSQueueSize); + + nav_sol_pub_ = + nh->advertise("navsol", kROSQueueSize); +} void UbloxFirmware6::getRosParams() { // Fix Service type, used when publishing fix status messages @@ -776,14 +793,10 @@ void UbloxFirmware6::fixDiagnostic( void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { if (enabled["nav_posllh"]) { - static ros::Publisher publisher = - nh->advertise("navposllh", kROSQueueSize); - publisher.publish(m); + nav_pos_llh_pub_.publish(m); } // Position message - static ros::Publisher fixPublisher = - nh->advertise("fix", kROSQueueSize); if (m.i_tow == last_nav_vel_.i_tow) { fix_.header.stamp = velocity_.header.stamp; // use last timestamp } else { @@ -812,7 +825,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_.status.service = fix_.status.SERVICE_GPS; - fixPublisher.publish(fix_); + fix_pub_.publish(fix_); last_nav_pos_ = m; // update diagnostics freq_diag->diagnostic->tick(fix_.header.stamp); @@ -821,15 +834,10 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { if (enabled["nav_velned"]) { - static ros::Publisher publisher = - nh->advertise("navvelned", kROSQueueSize); - publisher.publish(m); + nav_vel_ned_pub_.publish(m); } // Example geometry message - static ros::Publisher velocityPublisher = - nh->advertise("fix_velocity", - kROSQueueSize); if (m.i_tow == last_nav_pos_.i_tow) { velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh } else { @@ -850,15 +858,13 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { velocity_.twist.covariance[cols * 2 + 2] = var_speed; velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - velocityPublisher.publish(velocity_); + vel_pub_.publish(velocity_); last_nav_vel_ = m; } void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { if (enabled["nav_sol"]) { - static ros::Publisher publisher = - nh->advertise("navsol", kROSQueueSize); - publisher.publish(m); + nav_sol_pub_.publish(m); } last_nav_sol_ = m; } @@ -866,7 +872,8 @@ void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { // // Ublox Firmware Version 7 // -UbloxFirmware7::UbloxFirmware7() {} +UbloxFirmware7::UbloxFirmware7() { +} void UbloxFirmware7::getRosParams() { // @@ -880,22 +887,29 @@ void UbloxFirmware7::getRosParams() { ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); - if(enable_gps_ && !supportsGnss("GPS")) + if (enable_gps_ && !supportsGnss("GPS")) { ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - if(enable_glonass_ && !supportsGnss("GLO")) + } + if (enable_glonass_ && !supportsGnss("GLO")) { ROS_WARN("gnss/glonass is true, but GLONASS is not %s", "supported by this device"); - if(enable_qzss_ && !supportsGnss("QZSS")) + } + if (enable_qzss_ && !supportsGnss("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if(enable_sbas_ && !supportsGnss("SBAS")) + } + if (enable_sbas_ && !supportsGnss("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + } - if(nh->hasParam("gnss/galileo")) + if (nh->hasParam("gnss/galileo")) { ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - if(nh->hasParam("gnss/beidou")) + } + if (nh->hasParam("gnss/beidou")) { ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - if(nh->hasParam("gnss/imes")) + } + if (nh->hasParam("gnss/imes")) { ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); + } // Fix Service type, used when publishing fix status messages fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS @@ -908,21 +922,26 @@ void UbloxFirmware7::getRosParams() { if (set_nmea_) { bool compat, consider; - if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) + if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) + } + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) + } + if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); - if (!nh->getParam("nmea/compat", compat)) + } + if (!nh->getParam("nmea/compat", compat)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) + } + if (!nh->getParam("nmea/consider", consider)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/consider must be set"); + } // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -1339,6 +1358,14 @@ void RawDataProduct::initializeRosDiagnostics() { // // u-blox ADR devices, partially implemented // +AdrUdrProduct::AdrUdrProduct() +{ + imu_pub_ = + nh->advertise("imu_meas", kROSQueueSize); + time_ref_pub_ = + nh->advertise("interrupt_time", kROSQueueSize); +} + void AdrUdrProduct::getRosParams() { nh->param("use_adr", use_adr_, true); // Check the nav rate @@ -1401,11 +1428,6 @@ void AdrUdrProduct::subscribe() { void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { if (enabled["esf_meas"]) { - static ros::Publisher imu_pub = - nh->advertise("imu_meas", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; @@ -1490,8 +1512,8 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - time_ref_pub.publish(t_ref_); - imu_pub.publish(imu_); + time_ref_pub_.publish(t_ref_); + imu_pub_.publish(imu_); } } @@ -1500,37 +1522,51 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // // u-blox High Precision GNSS Reference Station // + +HpgRefProduct::HpgRefProduct() +{ + navsvin_pub_ = + nh->advertise("navsvin", kROSQueueSize); +} + void HpgRefProduct::getRosParams() { if (config_on_startup_flag_) { - if(nav_rate * meas_rate != 1000) + if (nav_rate * meas_rate != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); + } - if(!getRosUint("tmode3", tmode3_)) + if (!getRosUint("tmode3", tmode3_)) { throw std::runtime_error("Invalid settings: TMODE3 must be set"); + } - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!nh->getParam("arp/position", arp_position_)) + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!nh->getParam("arp/position", arp_position_)) { throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); - if(!getRosInt("arp/position_hp", arp_position_hp_)) + } + if (!getRosInt("arp/position_hp", arp_position_hp_)) { throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/acc", fixed_pos_acc_)) + } + if (!nh->getParam("arp/acc", fixed_pos_acc_)) { throw std::runtime_error(std::string("Invalid settings: arp/acc ") + "must be set if TMODE3 is fixed"); - if(!nh->getParam("arp/lla_flag", lla_flag_)) { + } + if (!nh->getParam("arp/lla_flag", lla_flag_)) { ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { nh->param("sv_in/reset", svin_reset_, true); - if(!getRosUint("sv_in/min_dur", sv_in_min_dur_)) + if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + "must be set if TMODE3 is survey-in"); - if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) + } + if (!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + "must be set if TMODE3 is survey-in"); + } } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + " flag constants for possible values."); @@ -1541,36 +1577,41 @@ void HpgRefProduct::getRosParams() { bool HpgRefProduct::configureUblox() { // Configure TMODE3 if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if(!gps.disableTmode3()) + if (!gps.disableTmode3()) { throw std::runtime_error("Failed to disable TMODE3."); + } mode_ = DISABLED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, - fixed_pos_acc_)) + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, + fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) + } + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) { throw std::runtime_error("Failed to set RTCM rates"); + } mode_ = FIXED; } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if(!svin_reset_) { + if (!svin_reset_) { ublox_msgs::NavSVIN nav_svin; - if(!gps.poll(nav_svin)) + if (!gps.poll(nav_svin)) { throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); + } // Don't reset survey-in if it's already active - if(nav_svin.active) { + if (nav_svin.active) { mode_ = SURVEY_IN; return true; } // Don't reset survey-in if it already has a valid value - if(nav_svin.valid) { + if (nav_svin.valid) { setTimeMode(); return true; } ublox_msgs::NavPVT nav_pvt; - if(!gps.poll(nav_pvt)) + if (!gps.poll(nav_pvt)) { throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); + } // Don't reset survey in if in time mode with a good fix if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { @@ -1582,20 +1623,24 @@ bool HpgRefProduct::configureUblox() { // For Survey in, meas rate must be at least 1 Hz uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] // If measurement period isn't a factor of 1000, set to default - if(1000 % meas_rate_temp != 0) + if (1000 % meas_rate_temp != 0) { meas_rate_temp = kDefaultMeasPeriod; + } // Set nav rate to 1 Hz during survey in - if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) + if (!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); + } // As recommended in the documentation, first disable, then set to survey in - if(!gps.disableTmode3()) + if (!gps.disableTmode3()) { ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); - else + } else { mode_ = DISABLED; + } // Set to Survey in mode - if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) + if (!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { throw std::runtime_error("Failed to set TMODE3 to survey-in."); + } mode_ = SURVEY_IN; } return true; @@ -1610,15 +1655,13 @@ void HpgRefProduct::subscribe() { } void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { - if(enabled["nav_svin"]) { - static ros::Publisher publisher = - nh->advertise("navsvin", kROSQueueSize); - publisher.publish(m); + if (enabled["nav_svin"]) { + navsvin_pub_.publish(m); } last_nav_svin_ = m; - if(!m.active && m.valid && mode_ == SURVEY_IN) { + if (!m.active && m.valid && mode_ == SURVEY_IN) { setTimeMode(); } @@ -1631,11 +1674,12 @@ bool HpgRefProduct::setTimeMode() { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if(!gps.configRate(meas_rate, nav_rate)) + if (!gps.configRate(meas_rate, nav_rate)) { ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, "navigation rate to ", nav_rate); + } // Enable the RTCM out messages - if(!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps.configRtcm(rtcm_ids, rtcm_rates)) { ROS_ERROR("Failed to configure RTCM IDs"); return false; } @@ -1692,6 +1736,12 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // +HpgRovProduct::HpgRovProduct() +{ + nav_rel_pos_ned_pub_ = + nh->advertise("navrelposned", kROSQueueSize); +} + void HpgRovProduct::getRosParams() { // default to float, see CfgDGNSS message for details getRosUint("dgnss_mode", dgnss_mode_, @@ -1759,9 +1809,7 @@ void HpgRovProduct::carrierPhaseDiagnostics( void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); - publisher.publish(m); + nav_rel_pos_ned_pub_.publish(m); } last_rel_pos_ = m; @@ -1771,6 +1819,14 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // +HpPosRecProduct::HpPosRecProduct() +{ + nav_relposned_pub_ = + nh->advertise("navrelposned", kROSQueueSize); + + imu_pub_ = + nh->advertise("navheading", kROSQueueSize); +} void HpPosRecProduct::subscribe() { // Whether to publish Nav Relative Position NED @@ -1785,15 +1841,10 @@ void HpPosRecProduct::subscribe() { void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { if (enabled["nav_relposned"]) { - static ros::Publisher publisher = - nh->advertise("navrelposned", kROSQueueSize); - publisher.publish(m); + nav_relposned_pub_.publish(m); } if (enabled["nav_heading"]) { - static ros::Publisher imu_pub = - nh->advertise("navheading", kROSQueueSize); - imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id; @@ -1812,7 +1863,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { imu_.orientation_covariance[4] = 1000.0; imu_.orientation_covariance[8] = pow(m.acc_heading / 10000.0, 2); - imu_pub.publish(imu_); + imu_pub_.publish(imu_); } last_rel_pos_ = m; @@ -1822,6 +1873,14 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // // U-Blox Time Sync Products, partially implemented. // +TimProduct::TimProduct() +{ + timtm2_pub_ = + nh->advertise("timtm2", kROSQueueSize); + interrupt_time_pub_ = + nh->advertise("interrupt_time", kROSQueueSize); +} + void TimProduct::getRosParams() { } @@ -1868,11 +1927,6 @@ void TimProduct::subscribe() { void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { if (enabled["tim_tm2"]) { - static ros::Publisher publisher = - nh->advertise("timtm2", kROSQueueSize); - static ros::Publisher time_ref_pub = - nh->advertise("interrupt_time", kROSQueueSize); - // create time ref message and put in the data t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); @@ -1887,8 +1941,8 @@ void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { t_ref_.header.stamp = ros::Time::now(); // create a new timestamp t_ref_.header.frame_id = frame_id; - publisher.publish(m); - time_ref_pub.publish(t_ref_); + timtm2_pub_.publish(m); + interrupt_time_pub_.publish(t_ref_); } updater->force_update(); diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 4093fe47..a13f18e4 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -52,142 +52,140 @@ RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) { + raw_pub_ = pnh_.advertise("raw_data_stream", 100); } void RawDataStreamPa::getRosParams() { - if (is_ros_subscriber_) { - pnh_.param("dir", file_dir_, ""); - } else { - pnh_.param("raw_data_stream/dir", file_dir_, ""); - pnh_.param("raw_data_stream/publish", flag_publish_, false); - } + if (is_ros_subscriber_) { + pnh_.param("dir", file_dir_, ""); + } else { + pnh_.param("raw_data_stream/dir", file_dir_, ""); + pnh_.param("raw_data_stream/publish", flag_publish_, false); + } } bool RawDataStreamPa::isEnabled() { - if (is_ros_subscriber_) { - return !file_dir_.empty(); - } else { - return flag_publish_ || (!file_dir_.empty()); - } + if (is_ros_subscriber_) { + return !file_dir_.empty(); + } else { + return flag_publish_ || (!file_dir_.empty()); + } } void RawDataStreamPa::initialize() { - if (is_ros_subscriber_) { - ROS_INFO("Subscribing to raw data stream."); - static ros::Subscriber subscriber = - nh_.subscribe ("raw_data_stream", 100, - &RawDataStreamPa::msgCallback, this); - } else if (flag_publish_) { - ROS_INFO("Publishing raw data stream."); - RawDataStreamPa::publishMsg(std::string()); - } + if (is_ros_subscriber_) { + ROS_INFO("Subscribing to raw data stream."); + static ros::Subscriber subscriber = + nh_.subscribe ("raw_data_stream", 100, + &RawDataStreamPa::msgCallback, this); + } else if (flag_publish_) { + ROS_INFO("Publishing raw data stream."); + RawDataStreamPa::publishMsg(std::string()); + } + + if (!file_dir_.empty()) { + struct stat stat_info; + if (stat(file_dir_.c_str(), &stat_info ) != 0) { + ROS_ERROR("Can't log raw data to file. " + "Directory \"%s\" does not exist.", file_dir_.c_str()); + + } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { + ROS_ERROR("Can't log raw data to file. " + "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - if (!file_dir_.empty()) { - struct stat stat_info; - if (stat(file_dir_.c_str(), &stat_info ) != 0) { - ROS_ERROR("Can't log raw data to file. " - "Directory \"%s\" does not exist.", file_dir_.c_str()); - - } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { - ROS_ERROR("Can't log raw data to file. " - "\"%s\" exists, but is not a directory.", file_dir_.c_str()); - - } else { - if (file_dir_.back() != '/') { - file_dir_ += '/'; - } - - time_t t = time(NULL); - struct tm time_struct = *localtime(&t); - - std::stringstream filename; - filename.width(4); filename.fill('0'); - filename << time_struct.tm_year + 1900; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mon + 1; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_mday; - filename.width(0); filename << '_'; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_hour; - filename.width(2); filename.fill('0'); - filename << time_struct.tm_min ; - filename.width(0); filename << ".log"; - file_name_ = file_dir_ + filename.str(); - - try { - file_handle_.open(file_name_); - ROS_INFO("Logging raw data to file \"%s\"", - file_name_.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Can't log raw data to file. " + } else { + if (file_dir_.back() != '/') { + file_dir_ += '/'; + } + + time_t t = time(NULL); + struct tm time_struct = *localtime(&t); + + std::stringstream filename; + filename.width(4); filename.fill('0'); + filename << time_struct.tm_year + 1900; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_mon + 1; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_mday; + filename.width(0); filename << '_'; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_hour; + filename.width(2); filename.fill('0'); + filename << time_struct.tm_min ; + filename.width(0); filename << ".log"; + file_name_ = file_dir_ + filename.str(); + + try { + file_handle_.open(file_name_); + ROS_INFO("Logging raw data to file \"%s\"", + file_name_.c_str()); + } catch(const std::exception& e) { + ROS_ERROR("Can't log raw data to file. " "Can't create file \"%s\".", file_name_.c_str()); - } - } + } } + } } void RawDataStreamPa::ubloxCallback(const unsigned char* data, const std::size_t size) { - std::string str((const char*) data, size); + std::string str((const char*) data, size); - if (flag_publish_) { - publishMsg(str); - } + if (flag_publish_) { + publishMsg(str); + } - saveToFile(str); + saveToFile(str); } void RawDataStreamPa::msgCallback( const std_msgs::UInt8MultiArray::ConstPtr& msg) { - std::string str(msg->data.size(), ' '); - std::copy(msg->data.begin(), msg->data.end(), str.begin()); - saveToFile(str); + std::string str(msg->data.size(), ' '); + std::copy(msg->data.begin(), msg->data.end(), str.begin()); + saveToFile(str); } std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( const std::string str) { - std_msgs::UInt8MultiArray msg; + std_msgs::UInt8MultiArray msg; - msg.layout.data_offset = 0; - msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); - msg.layout.dim[0].size = str.length(); - msg.layout.dim[0].stride = 1; - msg.layout.dim[0].label = "raw_data_stream"; + msg.layout.data_offset = 0; + msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.layout.dim[0].size = str.length(); + msg.layout.dim[0].stride = 1; + msg.layout.dim[0].label = "raw_data_stream"; - msg.data.resize(str.length()); - std::copy(str.begin(), str.end(), msg.data.begin()); + msg.data.resize(str.length()); + std::copy(str.begin(), str.end(), msg.data.begin()); - return msg; + return msg; } void RawDataStreamPa::publishMsg(const std::string str) { - static ros::Publisher publisher = - pnh_.advertise("raw_data_stream", 100); - - publisher.publish(RawDataStreamPa::str2uint8(str)); + raw_pub_.publish(RawDataStreamPa::str2uint8(str)); } void RawDataStreamPa::saveToFile(const std::string str) { - if (file_handle_.is_open()) { - try { - file_handle_ << str; - // file_handle_.flush(); - } catch(const std::exception& e) { - ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); - } + if (file_handle_.is_open()) { + try { + file_handle_ << str; + // file_handle_.flush(); + } catch(const std::exception& e) { + ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); } + } } } // namespace ublox_node From 309e628d2c7b23035331ef097deb358319524711 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 16:32:45 -0500 Subject: [PATCH 019/152] Switch to const references for std::string where possible. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 6 +-- ublox_gps/include/ublox_gps/node.hpp | 44 +++++++++++++-------- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 6 +-- ublox_gps/src/gps.cpp | 6 +-- ublox_gps/src/node.cpp | 4 +- ublox_gps/src/raw_data_pa.cpp | 6 +-- 6 files changed, 41 insertions(+), 31 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index b17b5c57..7295f2d8 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -93,7 +93,7 @@ class Gps { * @param host the TCP host * @param port the TCP port */ - void initializeTcp(std::string host, std::string port); + void initializeTcp(const std::string & host, const std::string & port); /** * @brief Initialize the Serial I/O port. @@ -102,7 +102,7 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void initializeSerial(std::string port, unsigned int baudrate, + void initializeSerial(const std::string & port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out); /** @@ -112,7 +112,7 @@ class Gps { * @param uart_in the UART In protocol, see CfgPRT for options * @param uart_out the UART Out protocol, see CfgPRT for options */ - void resetSerial(std::string port); + void resetSerial(const std::string & port); /** * @brief Closes the I/O port, and initiates save on shutdown procedure diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index c73c7199..b3ba39bd 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -137,7 +137,7 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) { + UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -158,7 +158,7 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max, + UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, double freq_tol, int freq_window) { min_freq = freq_min; max_freq = freq_max; @@ -255,8 +255,8 @@ uint8_t fixModeFromString(const std::string& mode); * @throws std::runtime_error if it is below the minimum */ template -void checkMin(V val, T min, std::string name) { - if(val < min) { +void checkMin(V val, T min, const std::string & name) { + if (val < min) { std::stringstream oss; oss << "Invalid settings: " << name << " must be > " << min; throw std::runtime_error(oss.str()); @@ -272,8 +272,8 @@ void checkMin(V val, T min, std::string name) { * @throws std::runtime_error if it is out of bounds */ template -void checkRange(V val, T min, T max, std::string name) { - if(val < min || val > max) { +void checkRange(V val, T min, T max, const std::string & name) { + if (val < min || val > max) { std::stringstream oss; oss << "Invalid settings: " << name << " must be in range [" << min << ", " << max << "]."; @@ -290,8 +290,8 @@ void checkRange(V val, T min, T max, std::string name) { * @throws std::runtime_error value it is out of bounds */ template -void checkRange(std::vector val, T min, T max, std::string name) { - for(size_t i = 0; i < val.size(); i++) { +void checkRange(std::vector val, T min, T max, const std::string & name) { + for (size_t i = 0; i < val.size(); i++) { std::stringstream oss; oss << name << "[" << i << "]"; checkRange(val[i], min, max, oss.str()); @@ -308,7 +308,9 @@ void checkRange(std::vector val, T min, T max, std::string name) { template bool getRosUint(const std::string& key, U &u) { int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) { + return false; + } // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); @@ -328,8 +330,9 @@ bool getRosUint(const std::string& key, U &u) { */ template void getRosUint(const std::string& key, U &u, V default_val) { - if(!getRosUint(key, u)) + if (!getRosUint(key, u)) { u = default_val; + } } /** @@ -340,7 +343,9 @@ void getRosUint(const std::string& key, U &u, V default_val) { template bool getRosUint(const std::string& key, std::vector &u) { std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) { + return false; + } // Check the bounds U min = std::numeric_limits::lowest(); @@ -362,7 +367,9 @@ bool getRosUint(const std::string& key, std::vector &u) { template bool getRosInt(const std::string& key, I &u) { int param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) { + return false; + } // Check the bounds I min = std::numeric_limits::lowest(); I max = std::numeric_limits::max(); @@ -382,8 +389,9 @@ bool getRosInt(const std::string& key, I &u) { */ template void getRosInt(const std::string& key, U &u, V default_val) { - if(!getRosInt(key, u)) + if (!getRosInt(key, u)) { u = default_val; + } } /** @@ -394,7 +402,9 @@ void getRosInt(const std::string& key, U &u, V default_val) { template bool getRosInt(const std::string& key, std::vector &i) { std::vector param; - if (!nh->getParam(key, param)) return false; + if (!nh->getParam(key, param)) { + return false; + } // Check the bounds I min = std::numeric_limits::lowest(); @@ -426,7 +436,7 @@ void publish(const MessageT& m, const std::string& topic) { * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES * @return true if the device supports the given GNSS */ -bool supportsGnss(std::string gnss) { +bool supportsGnss(const std::string & gnss) { return supported.count(gnss) > 0; } @@ -572,8 +582,8 @@ class UbloxNode : public virtual ComponentInterface { * @param for HPG/TIM products, this value is either REF or ROV, for other * products this string is empty */ - void addProductInterface(std::string product_category, - std::string ref_rov = ""); + void addProductInterface(const std::string & product_category, + const std::string & ref_rov = ""); /** * @brief Poll messages from the U-Blox device. diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 2fd22c37..0463cd56 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -104,19 +104,19 @@ class RawDataStreamPa { /** * @brief Converts a string into an uint8 multibyte array */ - std_msgs::UInt8MultiArray str2uint8(const std::string str); + std_msgs::UInt8MultiArray str2uint8(const std::string & str); /** * @brief Publishes data stream as ros message * @param str raw data stream as string */ - void publishMsg(const std::string str); + void publishMsg(const std::string & str); /** * @brief Stores data to given file * @param str raw data stream as string */ - void saveToFile(const std::string str); + void saveToFile(const std::string & str); //! Directoy name for storing raw data std::string file_dir_; diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index d79a25bf..f9cb6c57 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -113,7 +113,7 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { } } -void Gps::initializeSerial(std::string port, unsigned int baudrate, +void Gps::initializeSerial(const std::string & port, unsigned int baudrate, uint16_t uart_in, uint16_t uart_out) { port_ = port; auto io_service = std::make_shared(); @@ -172,7 +172,7 @@ void Gps::initializeSerial(std::string port, unsigned int baudrate, } } -void Gps::resetSerial(std::string port) { +void Gps::resetSerial(const std::string & port) { auto io_service = std::make_shared(); auto serial = std::make_shared(*io_service); @@ -212,7 +212,7 @@ void Gps::resetSerial(std::string port) { configured_ = true; } -void Gps::initializeTcp(std::string host, std::string port) { +void Gps::initializeTcp(const std::string & host, const std::string & port) { host_ = host; port_ = port; auto io_service = std::make_shared(); diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 231b27dd..534eeb11 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -134,8 +134,8 @@ void UbloxNode::addFirmwareInterface() { } -void UbloxNode::addProductInterface(std::string product_category, - std::string ref_rov) { +void UbloxNode::addProductInterface(const std::string & product_category, + const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) components_.push_back(std::make_shared()); else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index a13f18e4..52208c51 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -155,7 +155,7 @@ void RawDataStreamPa::msgCallback( } std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( - const std::string str) { + const std::string & str) { std_msgs::UInt8MultiArray msg; @@ -171,12 +171,12 @@ std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( return msg; } -void RawDataStreamPa::publishMsg(const std::string str) { +void RawDataStreamPa::publishMsg(const std::string & str) { raw_pub_.publish(RawDataStreamPa::str2uint8(str)); } -void RawDataStreamPa::saveToFile(const std::string str) { +void RawDataStreamPa::saveToFile(const std::string & str) { if (file_handle_.is_open()) { try { From 05fe25ebf2df83f18f254dab2fab806049d40212 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 17:31:25 -0500 Subject: [PATCH 020/152] Remove last static advertiser. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 6 +- ublox_gps/include/ublox_gps/gps.hpp | 16 +- ublox_gps/include/ublox_gps/node.hpp | 51 +++- ublox_gps/src/node.cpp | 304 ++++++++++++++--------- 4 files changed, 249 insertions(+), 128 deletions(-) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index f0ef963e..5956954a 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -110,7 +110,9 @@ class CallbackHandler_ : public CallbackHandler { return; } - if (func_) func_(message_); + if (func_) { + func_(message_); + } condition_.notify_all(); } @@ -217,7 +219,7 @@ class CallbackHandlers { oss << std::hex << static_cast(*it) << " "; } ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, - oss.str().c_str()); + oss.str().c_str()); } handle(reader); diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 7295f2d8..8df76585 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -482,7 +482,9 @@ class Gps { template void Gps::subscribe( typename CallbackHandler_::Callback callback, unsigned int rate) { - if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) return; + if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) { + return; + } subscribe(callback); } @@ -501,19 +503,25 @@ template bool Gps::poll(ConfigT& message, const std::vector& payload, const std::chrono::milliseconds& timeout) { - if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) return false; + if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) { + return false; + } return read(message, timeout); } template bool Gps::read(T& message, const std::chrono::milliseconds& timeout) { - if (!worker_) return false; + if (!worker_) { + return false; + } return callbacks_.read(message, timeout); } template bool Gps::configure(const ConfigT& message, bool wait) { - if (!worker_) return false; + if (!worker_) { + return false; + } // Reset ack Ack ack; diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index b3ba39bd..272a715e 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -425,9 +425,7 @@ bool getRosInt(const std::string& key, std::vector &i) { * @param topic the topic to publish the message on */ template -void publish(const MessageT& m, const std::string& topic) { - static ros::Publisher publisher = nh->advertise(topic, - kROSQueueSize); +void publish(const MessageT& m, ros::Publisher & publisher) { publisher.publish(m); } @@ -656,6 +654,13 @@ class UbloxNode : public virtual ComponentInterface { //! raw data stream logging RawDataStreamPa rawDataStreamPa_; + + ros::Publisher nav_status_pub_; + ros::Publisher nav_posecef_pub_; + ros::Publisher nav_clock_pub_; + ros::Publisher aid_alm_pub_; + ros::Publisher aid_eph_pub_; + ros::Publisher aid_hui_pub_; }; /** @@ -755,6 +760,8 @@ class UbloxFirmware6 : public UbloxFirmware { ros::Publisher nav_vel_ned_pub_; ros::Publisher vel_pub_; ros::Publisher nav_sol_pub_; + ros::Publisher nav_svinfo_pub_; + ros::Publisher mon_hw_pub_; }; /** @@ -970,14 +977,17 @@ class UbloxFirmware7 : public UbloxFirmware7Plus { */ void subscribe(); - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; + private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::CfgNMEA7 cfg_nmea_; + //! Whether or not to Configure the NMEA settings + bool set_nmea_; + + ros::Publisher nav_svinfo_pub_; + ros::Publisher mon_hw_pub_; }; /** @@ -1026,6 +1036,10 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { ublox_msgs::CfgNMEA cfg_nmea_; //! Whether to clear the flash memory during configuration bool clear_bbr_; + + ros::Publisher nav_sat_pub_; + ros::Publisher mon_hw_pub_; + ros::Publisher rxm_rtcm_pub_; }; /** @@ -1044,6 +1058,8 @@ class RawDataProduct: public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; + RawDataProduct(); + /** * @brief Does nothing since there are no Raw Data product specific settings. */ @@ -1070,6 +1086,11 @@ class RawDataProduct: public virtual ComponentInterface { private: //! Topic diagnostic updaters std::vector > freq_diagnostics_; + + ros::Publisher rxm_raw_pub_; + ros::Publisher rxm_sfrb_pub_; + ros::Publisher rxm_eph_pub_; + ros::Publisher rxm_alm_pub_; }; /** @@ -1123,6 +1144,12 @@ class AdrUdrProduct: public virtual ComponentInterface { ros::Publisher imu_pub_; ros::Publisher time_ref_pub_; + ros::Publisher nav_att_pub_; + ros::Publisher esf_ins_pub_; + ros::Publisher esf_meas_pub_; + ros::Publisher esf_raw_pub_; + ros::Publisher esf_status_pub_; + ros::Publisher hnr_pvt_pub_; }; /** @@ -1414,6 +1441,8 @@ class TimProduct: public virtual ComponentInterface { ros::Publisher timtm2_pub_; ros::Publisher interrupt_time_pub_; + ros::Publisher rxm_sfrb_pub_; + ros::Publisher rxm_raw_pub_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 534eeb11..0f4b54f5 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -47,23 +47,23 @@ namespace ublox_node { uint8_t modelFromString(const std::string& model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); - if(lower == "portable") { + if (lower == "portable") { return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; - } else if(lower == "stationary") { + } else if (lower == "stationary") { return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; - } else if(lower == "pedestrian") { + } else if (lower == "pedestrian") { return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if(lower == "automotive") { + } else if (lower == "automotive") { return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if(lower == "sea") { + } else if (lower == "sea") { return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; - } else if(lower == "airborne1") { + } else if (lower == "airborne1") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if(lower == "airborne2") { + } else if (lower == "airborne2") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if(lower == "airborne4") { + } else if (lower == "airborne4") { return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if(lower == "wristwatch") { + } else if (lower == "wristwatch") { return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; } @@ -111,6 +111,12 @@ std::vector stringSplit(const std::string &str, // u-blox ROS Node // UbloxNode::UbloxNode() { + nav_status_pub_ = nh->advertise("navstatus", kROSQueueSize); + nav_posecef_pub_ = nh->advertise("navposecef", kROSQueueSize); + nav_clock_pub_ = nh->advertise("navclock", kROSQueueSize); + aid_alm_pub_ = nh->advertise("aidalm", kROSQueueSize); + aid_eph_pub_ = nh->advertise("aideph", kROSQueueSize); + aid_hui_pub_ = nh->advertise("aidhui", kROSQueueSize); initialize(); } @@ -136,23 +142,24 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { - if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) + if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { components_.push_back(std::make_shared()); - else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) + } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared()); - else if (product_category.compare("HPG") == 0) + } else if (product_category.compare("HPG") == 0) { components_.push_back(std::make_shared()); - else if (product_category.compare("TIM") == 0) + } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared()); - else if (product_category.compare("ADR") == 0 || - product_category.compare("UDR") == 0) + } else if (product_category.compare("ADR") == 0 || + product_category.compare("UDR") == 0) { components_.push_back(std::make_shared()); - else if (product_category.compare("FTS") == 0) + } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); - else if(product_category.compare("SPG") != 0) + } else if (product_category.compare("SPG") != 0) { ROS_WARN("Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); + } } void UbloxNode::getRosParams() { @@ -175,11 +182,11 @@ void UbloxNode::getRosParams() { set_usb_ = false; if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { set_usb_ = true; - if(!getRosUint("usb/in", usb_in_)) { + if (!getRosUint("usb/in", usb_in_)) { throw std::runtime_error(std::string("usb/out is set, therefore ") + "usb/in must be set"); } - if(!getRosUint("usb/out", usb_out_)) { + if (!getRosUint("usb/out", usb_out_)) { throw std::runtime_error(std::string("usb/in is set, therefore ") + "usb/out must be set"); } @@ -201,31 +208,35 @@ void UbloxNode::getRosParams() { nh->param("fix_mode", fix_mode_, std::string("auto")); getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit - if (enable_ppp_) + if (enable_ppp_) { ROS_WARN("Warning: PPP is enabled - this is an expert setting."); + } checkMin(rate_, 0, "rate"); - if(rtcm_ids.size() != rtcm_rates.size()) + if (rtcm_ids.size() != rtcm_rates.size()) { throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); + } dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); nh->param("dat/set", set_dat_, false); - if(set_dat_) { + if (set_dat_) { std::vector shift, rot; if (!nh->getParam("dat/majA", cfg_dat_.maj_a) || nh->getParam("dat/flat", cfg_dat_.flat) || nh->getParam("dat/shift", shift) || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) + || nh->getParam("dat/scale", cfg_dat_.scale)) { throw std::runtime_error(std::string("dat/set is true, therefore ") + "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); - if(shift.size() != 3 || rot.size() != 3) + } + if (shift.size() != 3 || rot.size() != 3) { throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + "must be 3"); + } checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat/majA"); checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); @@ -254,12 +265,15 @@ void UbloxNode::getRosParams() { void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); - if (enabled["aid_alm"]) + if (enabled["aid_alm"]) { gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); - if (enabled["aid_eph"]) + } + if (enabled["aid_eph"]) { gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); - if (enabled["aid_hui"]) + } + if (enabled["aid_hui"]) { gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); + } payload[0]++; if (payload[0] > 32) { @@ -268,14 +282,15 @@ void UbloxNode::pollMessages(const ros::TimerEvent& event) { } void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { - if (id == ublox_msgs::Message::INF::ERROR) + if (id == ublox_msgs::Message::INF::ERROR) { ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else if (id == ublox_msgs::Message::INF::WARNING) + } else if (id == ublox_msgs::Message::INF::WARNING) { ROS_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else if (id == ublox_msgs::Message::INF::DEBUG) + } else if (id == ublox_msgs::Message::INF::DEBUG) { ROS_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); - else + } else { ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); + } } void UbloxNode::subscribe() { @@ -290,71 +305,82 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); - if (enabled["nav_status"]) + if (enabled["nav_status"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navstatus"), kSubscribeRate); + publish, std::placeholders::_1, nav_status_pub_), kSubscribeRate); + } nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); - if (enabled["nav_posecef"]) + if (enabled["nav_posecef"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navposecef"), kSubscribeRate); + publish, std::placeholders::_1, nav_posecef_pub_), kSubscribeRate); + } nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); - if (enabled["nav_clock"]) + if (enabled["nav_clock"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navclock"), kSubscribeRate); + publish, std::placeholders::_1, nav_clock_pub_), kSubscribeRate); + } // INF messages nh->param("inf/debug", enabled["inf_debug"], false); - if (enabled["inf_debug"]) + if (enabled["inf_debug"]) { gps.subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); + } nh->param("inf/error", enabled["inf_error"], enabled["inf"]); - if (enabled["inf_error"]) + if (enabled["inf_error"]) { gps.subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); + } nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); - if (enabled["inf_notice"]) + if (enabled["inf_notice"]) { gps.subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); + } nh->param("inf/test", enabled["inf_test"], enabled["inf"]); - if (enabled["inf_test"]) + if (enabled["inf_test"]) { gps.subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); + } nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); - if (enabled["inf_warning"]) + if (enabled["inf_warning"]) { gps.subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); + } // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); - if (enabled["aid_alm"]) + if (enabled["aid_alm"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "aidalm"), kSubscribeRate); + publish, std::placeholders::_1, aid_alm_pub_), kSubscribeRate); + } nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); - if (enabled["aid_eph"]) + if (enabled["aid_eph"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "aideph"), kSubscribeRate); + publish, std::placeholders::_1, aid_eph_pub_), kSubscribeRate); + } nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); - if (enabled["aid_hui"]) + if (enabled["aid_hui"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "aidhui"), kSubscribeRate); + publish, std::placeholders::_1, aid_hui_pub_), kSubscribeRate); + } for (size_t i = 0; i < components_.size(); i++) { components_[i]->subscribe(); @@ -377,7 +403,6 @@ void UbloxNode::initializeRosDiagnostics() { } } - void UbloxNode::processMonVer() { ublox_msgs::MonVER monVer; if (!gps.poll(monVer)) { @@ -422,7 +447,7 @@ void UbloxNode::processMonVer() { supported.insert(strs[i]); } } else { - for(std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extension.size(); ++i) { std::vector strs; // Up to 2nd to last line if (i <= extension.size() - 2) { @@ -595,7 +620,7 @@ void UbloxNode::initialize() { initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if(protocol_version_ <= 14) { + if (protocol_version_ <= 14) { if (nh->param("raw_data", false)) { components_.push_back(std::make_shared()); } @@ -647,17 +672,23 @@ UbloxFirmware6::UbloxFirmware6() nav_pos_llh_pub_ = nh->advertise("navposllh", kROSQueueSize); fix_pub_ = - nh->advertise("fix", kROSQueueSize); + nh->advertise("fix", kROSQueueSize); nav_vel_ned_pub_ = nh->advertise("navvelned", kROSQueueSize); vel_pub_ = - nh->advertise("fix_velocity", - kROSQueueSize); + nh->advertise("fix_velocity", + kROSQueueSize); + + nav_sol_pub_ = + nh->advertise("navsol", kROSQueueSize); + + nav_svinfo_pub_ = + nh->advertise("navinfo", kROSQueueSize); - nav_sol_pub_ = - nh->advertise("navsol", kROSQueueSize); + mon_hw_pub_ = + nh->advertise("monhw", kROSQueueSize); } void UbloxFirmware6::getRosParams() { @@ -737,7 +768,7 @@ void UbloxFirmware6::subscribe() { nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navsvinfo"), + publish, std::placeholders::_1, nav_svinfo_pub_), kNavSvInfoSubscribeRate); } @@ -745,7 +776,7 @@ void UbloxFirmware6::subscribe() { nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "monhw"), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); } } @@ -873,6 +904,8 @@ void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { // Ublox Firmware Version 7 // UbloxFirmware7::UbloxFirmware7() { + nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); + mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); } void UbloxFirmware7::getRosParams() { @@ -1022,7 +1055,7 @@ bool UbloxFirmware7::configureUblox() { } } - if(supportsGnss("SBAS")) { + if (supportsGnss("SBAS")) { // configure SBAS ublox_msgs::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_SBAS; @@ -1055,22 +1088,28 @@ void UbloxFirmware7::subscribe() { // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); - if (enabled["nav_svinfo"]) + if (enabled["nav_svinfo"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navsvinfo"), + publish, std::placeholders::_1, nav_svinfo_pub_), kNavSvInfoSubscribeRate); + } // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) + if (enabled["mon_hw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "monhw"), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); + } } // // Ublox Version 8 // -UbloxFirmware8::UbloxFirmware8() {} +UbloxFirmware8::UbloxFirmware8() { + nav_sat_pub_ = nh->advertise("navsate", kROSQueueSize); + mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); + rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); +} void UbloxFirmware8::getRosParams() { // UPD SOS configuration @@ -1089,25 +1128,32 @@ void UbloxFirmware8::getRosParams() { getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); - if (enable_gps_ && !supportsGnss("GPS")) + if (enable_gps_ && !supportsGnss("GPS")) { ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", "this device"); - if (enable_glonass_ && !supportsGnss("GLO")) + } + if (enable_glonass_ && !supportsGnss("GLO")) { ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", "this device"); - if (enable_galileo_ && !supportsGnss("GAL")) + } + if (enable_galileo_ && !supportsGnss("GAL")) { ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", "by this device"); - if (enable_beidou_ && !supportsGnss("BDS")) + } + if (enable_beidou_ && !supportsGnss("BDS")) { ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", "by this device"); - if (enable_imes_ && !supportsGnss("IMES")) + } + if (enable_imes_ && !supportsGnss("IMES")) { ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", "this device"); - if (enable_qzss_ && !supportsGnss("QZSS")) + } + if (enable_qzss_ && !supportsGnss("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - if (enable_sbas_ && !supportsGnss("SBAS")) + } + if (enable_sbas_ && !supportsGnss("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + } // Fix Service type, used when publishing fix status messages fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS @@ -1124,21 +1170,26 @@ void UbloxFirmware8::getRosParams() { cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set - if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) + if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) + } + if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) + } + if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); - if (!nh->getParam("nmea/compat", compat)) + } + if (!nh->getParam("nmea/compat", compat)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/compat must be set"); - if (!nh->getParam("nmea/consider", consider)) + } + if (!nh->getParam("nmea/consider", consider)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/consider must be set"); + } // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; @@ -1249,9 +1300,10 @@ bool UbloxFirmware8::configureUblox() { cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); - if (enable_qzss_) + if (enable_qzss_) { // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; + } } else if (block.gnss_id == block.GNSS_ID_GLONASS && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; @@ -1263,17 +1315,19 @@ bool UbloxFirmware8::configureUblox() { // If the GNSS is already configured correctly, do not re-configure GNSS // since this requires a cold reset - if (correct) + if (correct) { ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - else if (!gps.configGnss(cfg_gnss, std::chrono::seconds(15))) + } else if (!gps.configGnss(cfg_gnss, std::chrono::seconds(15))) { throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); + } // // NMEA config // - if (set_nmea_ && !gps.configure(cfg_nmea_)) + if (set_nmea_ && !gps.configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); + } return true; } @@ -1287,53 +1341,67 @@ void UbloxFirmware8::subscribe() { // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); - if (enabled["nav_sat"]) + if (enabled["nav_sat"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navsat"), kNavSvInfoSubscribeRate); + publish, std::placeholders::_1, nav_sat_pub_), kNavSvInfoSubscribeRate); + } // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); - if (enabled["mon_hw"]) + if (enabled["mon_hw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "monhw"), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); + } // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); - if (enabled["rxm_rtcm"]) + if (enabled["rxm_rtcm"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmrtcm"), kSubscribeRate); + publish, std::placeholders::_1, rxm_rtcm_pub_), kSubscribeRate); + } } // // Raw Data Products // +RawDataProduct::RawDataProduct() { + rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); + rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); + rxm_eph_pub_ = nh->advertise("rxmeph", kROSQueueSize); + rxm_alm_pub_ = nh->advertise("rxmalm", kROSQueueSize); +} + void RawDataProduct::subscribe() { // Defaults to true instead of to all nh->param("publish/rxm/all", enabled["rxm"], true); // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); - if (enabled["rxm_raw"]) + if (enabled["rxm_raw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmraw"), kSubscribeRate); + publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); + } // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); - if (enabled["rxm_sfrb"]) + if (enabled["rxm_sfrb"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmsfrb"), kSubscribeRate); + publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); + } // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); - if (enabled["rxm_eph"]) + if (enabled["rxm_eph"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmeph"), kSubscribeRate); + publish, std::placeholders::_1, rxm_eph_pub_), kSubscribeRate); + } // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); - if (enabled["rxm_alm"]) + if (enabled["rxm_alm"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmalm"), kSubscribeRate); + publish, std::placeholders::_1, rxm_alm_pub_), kSubscribeRate); + } } void RawDataProduct::initializeRosDiagnostics() { @@ -1364,6 +1432,12 @@ AdrUdrProduct::AdrUdrProduct() nh->advertise("imu_meas", kROSQueueSize); time_ref_pub_ = nh->advertise("interrupt_time", kROSQueueSize); + nav_att_pub_ = nh->advertise("navatt", kROSQueueSize); + esf_ins_pub_ = nh->advertise("esfins", kROSQueueSize); + esf_meas_pub_ = nh->advertise("esfmeas", kROSQueueSize); + esf_raw_pub_ = nh->advertise("esfraw", kROSQueueSize); + esf_status_pub_ = nh->advertise("esfstatus", kROSQueueSize); + hnr_pvt_pub_ = nh->advertise("hnrpvt", kROSQueueSize); } void AdrUdrProduct::getRosParams() { @@ -1388,42 +1462,48 @@ void AdrUdrProduct::subscribe() { // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); - if (enabled["nav_att"]) + if (enabled["nav_att"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "navatt"), kSubscribeRate); + publish, std::placeholders::_1, nav_att_pub_), kSubscribeRate); + } // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); - if (enabled["esf_ins"]) + if (enabled["esf_ins"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "esfins"), kSubscribeRate); + publish, std::placeholders::_1, esf_ins_pub_), kSubscribeRate); + } // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); - if (enabled["esf_meas"]) + if (enabled["esf_meas"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "esfmeas"), kSubscribeRate); + publish, std::placeholders::_1, esf_meas_pub_), kSubscribeRate); // also publish sensor_msgs::Imu gps.subscribe(std::bind( &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), kSubscribeRate); + } // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); - if (enabled["esf_raw"]) + if (enabled["esf_raw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "esfraw"), kSubscribeRate); + publish, std::placeholders::_1, esf_raw_pub_), kSubscribeRate); + } // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); - if (enabled["esf_status"]) + if (enabled["esf_status"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "esfstatus"), kSubscribeRate); + publish, std::placeholders::_1, esf_status_pub_), kSubscribeRate); + } // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); - if (enabled["hnr_pvt"]) + if (enabled["hnr_pvt"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "hnrpvt"), kSubscribeRate); + publish, std::placeholders::_1, hnr_pvt_pub_), kSubscribeRate); + } } void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { @@ -1567,7 +1647,7 @@ void HpgRefProduct::getRosParams() { throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + "must be set if TMODE3 is survey-in"); } - } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + } else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + " flag constants for possible values."); } @@ -1576,7 +1656,7 @@ void HpgRefProduct::getRosParams() { bool HpgRefProduct::configureUblox() { // Configure TMODE3 - if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { if (!gps.disableTmode3()) { throw std::runtime_error("Failed to disable TMODE3."); } @@ -1590,7 +1670,7 @@ bool HpgRefProduct::configureUblox() { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; - } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { if (!svin_reset_) { ublox_msgs::NavSVIN nav_svin; if (!gps.poll(nav_svin)) { @@ -1879,6 +1959,8 @@ TimProduct::TimProduct() nh->advertise("timtm2", kROSQueueSize); interrupt_time_pub_ = nh->advertise("interrupt_time", kROSQueueSize); + rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); + rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); } void TimProduct::getRosParams() { @@ -1913,14 +1995,14 @@ void TimProduct::subscribe() { nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmsfrb"), kSubscribeRate); + publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); } // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { gps.subscribe(std::bind( - publish, std::placeholders::_1, "rxmraw"), kSubscribeRate); + publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); } } From fa067a9166c9acd128cf23417e4b46bb6e94430f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 17:37:53 -0500 Subject: [PATCH 021/152] Remove the one static ros::Subscriber. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 7 ++++--- ublox_gps/src/node.cpp | 2 +- ublox_gps/src/raw_data_pa.cpp | 5 ++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 0463cd56..f4773d21 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -70,12 +70,12 @@ class RawDataStreamPa { /** * @brief Get the raw data stream parameters. */ - void getRosParams(void); + void getRosParams(); /** * @brief Returns the if raw data streaming is enabled. */ - bool isEnabled(void); + bool isEnabled(); /** * @brief Initializes raw data streams @@ -84,7 +84,7 @@ class RawDataStreamPa { * If publishing ros messages is enabled, an empty msg will be published. * (This will implicitly create the publisher) */ - void initialize(void); + void initialize(); /** * @brief Callback function which handles raw data. @@ -139,6 +139,7 @@ class RawDataStreamPa { ros::NodeHandle nh_; ros::Publisher raw_pub_; + ros::Subscriber raw_data_stream_sub_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 0f4b54f5..c09edfea 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -609,7 +609,7 @@ void UbloxNode::initializeIo() { // raw data stream logging if (rawDataStreamPa_.isEnabled()) { gps.setRawDataCallback( - std::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); + std::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); rawDataStreamPa_.initialize(); } } diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 52208c51..86792d08 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -74,13 +74,12 @@ bool RawDataStreamPa::isEnabled() { } } - void RawDataStreamPa::initialize() { if (is_ros_subscriber_) { ROS_INFO("Subscribing to raw data stream."); - static ros::Subscriber subscriber = - nh_.subscribe ("raw_data_stream", 100, + raw_data_stream_sub_ = + nh_.subscribe("raw_data_stream", 100, &RawDataStreamPa::msgCallback, this); } else if (flag_publish_) { ROS_INFO("Publishing raw data stream."); From 16b5901918747ec4e7b8d1deb7ba93873b5310d9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 17:47:18 -0500 Subject: [PATCH 022/152] Move ComponentInterface to its own file. Signed-off-by: Chris Lalancette --- .../include/ublox_gps/component_interface.hpp | 72 +++++++++++++++++++ ublox_gps/include/ublox_gps/node.hpp | 41 +---------- 2 files changed, 73 insertions(+), 40 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/component_interface.hpp diff --git a/ublox_gps/include/ublox_gps/component_interface.hpp b/ublox_gps/include/ublox_gps/component_interface.hpp new file mode 100644 index 00000000..3e973ff5 --- /dev/null +++ b/ublox_gps/include/ublox_gps/component_interface.hpp @@ -0,0 +1,72 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// 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 the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 THE COPYRIGHT HOLDER 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 UBLOX_GPS_COMPONENT_INTERFACE_HPP +#define UBLOX_GPS_COMPONENT_INTERFACE_HPP + +// This file declares the ComponentInterface which acts as a high level +// interface for u-blox firmware, product categories, etc. It contains methods +// to configure the u-blox and subscribe to u-blox messages. +// + +/** + * @brief This interface is used to add functionality to the main node. + * + * @details This interface is generic and can be implemented for other features + * besides the main node, hardware versions, and firmware versions. + */ +class ComponentInterface { + public: + /** + * @brief Get the ROS parameters. + * @throws std::runtime_error if a parameter is invalid or required + * parameters are not set. + */ + virtual void getRosParams() = 0; + + /** + * @brief Configure the U-Blox settings. + * @return true if configured correctly, false otherwise + */ + virtual bool configureUblox() = 0; + + /** + * @brief Initialize the diagnostics. + * + * @details Function may be empty. + */ + virtual void initializeRosDiagnostics() = 0; + + /** + * @brief Subscribe to u-blox messages and publish to ROS topics. + */ + virtual void subscribe() = 0; +}; + +#endif diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 272a715e..10af0058 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -49,14 +49,11 @@ // Other U-Blox package includes #include // Ublox GPS includes +#include #include #include #include -// This file declares the ComponentInterface which acts as a high level -// interface for u-blox firmware, product categories, etc. It contains methods -// to configure the u-blox and subscribe to u-blox messages. -// // This file also declares UbloxNode which implements ComponentInterface and is // the main class and ros node. it implements functionality which applies to // any u-blox device, regardless of the firmware version or product type. @@ -438,42 +435,6 @@ bool supportsGnss(const std::string & gnss) { return supported.count(gnss) > 0; } -/** - * @brief This interface is used to add functionality to the main node. - * - * @details This interface is generic and can be implemented for other features - * besides the main node, hardware versions, and firmware versions. - */ -class ComponentInterface { - public: - /** - * @brief Get the ROS parameters. - * @throws std::runtime_error if a parameter is invalid or required - * parameters are not set. - */ - virtual void getRosParams() = 0; - - /** - * @brief Configure the U-Blox settings. - * @return true if configured correctly, false otherwise - */ - virtual bool configureUblox() = 0; - - /** - * @brief Initialize the diagnostics. - * - * @details Function may be empty. - */ - virtual void initializeRosDiagnostics() = 0; - - /** - * @brief Subscribe to u-blox messages and publish to ROS topics. - */ - virtual void subscribe() = 0; -}; - -typedef std::shared_ptr ComponentPtr; - /** * @brief This class represents u-blox ROS node for *all* firmware and product * versions. From 04c7b095acb665073cd2737abb0c6fea0074927e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 17:49:46 -0500 Subject: [PATCH 023/152] UbloxNode is not a component. So don't make it a sub-class of ComponentInterface. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 10af0058..2155616f 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -450,7 +450,7 @@ bool supportsGnss(const std::string & gnss) { * The UbloxNode calls the public methods of ComponentInterface for each * element in the components vector. */ -class UbloxNode : public virtual ComponentInterface { +class UbloxNode final { public: //! How long to wait during I/O reset [s] constexpr static int kResetWait = 10; From dfbafa6d80eb75cd5af360fe41f59e4bab21d99b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2019 17:53:23 -0500 Subject: [PATCH 024/152] Move FTSProduct class into its own file. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/fts_product.hpp | 70 +++++++++++++++++++++ ublox_gps/include/ublox_gps/node.hpp | 37 +---------- ublox_gps/src/node.cpp | 1 + 3 files changed, 73 insertions(+), 35 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/fts_product.hpp diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp new file mode 100644 index 00000000..898474ab --- /dev/null +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -0,0 +1,70 @@ +//============================================================================== +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// 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 the Flight Systems and Automatic Control group, +// TU Darmstadt, 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 THE COPYRIGHT HOLDER 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 UBLOX_GPS_FTS_PRODUCT_HPP +#define UBLOX_GPS_FTS_PRODUCT_HPP + +#include + +#include + +/** + * @brief Implements functions for FTS products. Currently unimplemented. + * @todo Unimplemented. + */ +class FtsProduct final : public virtual ComponentInterface { + /** + * @brief Get the FTS parameters. + * @todo Currently unimplemented. + */ + void getRosParams() { + ROS_WARN("Functionality specific to u-blox FTS devices is %s", + "unimplemented. See FtsProduct class in node.hpp & node.cpp."); + } + + /** + * @brief Configure FTS settings. + * @todo Currently unimplemented. + */ + bool configureUblox() { return false; } + + /** + * @brief Subscribe to FTS messages. + * @todo Currently unimplemented. + */ + void subscribe() {} + + /** + * @brief Adds diagnostic updaters for FTS status. + * @todo Currently unimplemented. + */ + void initializeRosDiagnostics() {} +}; + +#endif diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 2155616f..37395adb 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -190,8 +190,8 @@ struct FixDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics * @param stamp_min the minimum allowed time delay */ - FixDiagnostic (std::string name, double freq_tol, int freq_window, - double stamp_min) { + FixDiagnostic(const std::string & name, double freq_tol, int freq_window, + double stamp_min) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -1113,39 +1113,6 @@ class AdrUdrProduct: public virtual ComponentInterface { ros::Publisher hnr_pvt_pub_; }; -/** - * @brief Implements functions for FTS products. Currently unimplemented. - * @todo Unimplemented. - */ -class FtsProduct: public virtual ComponentInterface { - /** - * @brief Get the FTS parameters. - * @todo Currently unimplemented. - */ - void getRosParams() { - ROS_WARN("Functionality specific to u-blox FTS devices is %s", - "unimplemented. See FtsProduct class in node.hpp & node.cpp."); - } - - /** - * @brief Configure FTS settings. - * @todo Currently unimplemented. - */ - bool configureUblox() { return false; } - - /** - * @brief Subscribe to FTS messages. - * @todo Currently unimplemented. - */ - void subscribe() {} - - /** - * @brief Adds diagnostic updaters for FTS status. - * @todo Currently unimplemented. - */ - void initializeRosDiagnostics() {} -}; - /** * @brief Implements functions for High Precision GNSS Reference station * devices. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index c09edfea..3e00d8ef 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -37,6 +37,7 @@ #include +#include #include "ublox_gps/node.hpp" namespace ublox_node { From f1f9c261a8f27dfb528d64add9c52c0d9fb69f8b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 14:04:29 +0000 Subject: [PATCH 025/152] Move the directory structure around just a bit. Signed-off-by: Chris Lalancette --- .../ublox/serialization/ublox_msgs.hpp | 2 +- .../checksum.hpp | 10 +++++----- .../serialization.hpp | 20 +++++++++++-------- .../serialization_ros.hpp | 14 +++++++------ 4 files changed, 26 insertions(+), 20 deletions(-) rename ublox_serialization/include/{ublox => ublox_serialization}/checksum.hpp (94%) rename ublox_serialization/include/{ublox => ublox_serialization}/serialization.hpp (98%) rename ublox_serialization/include/{ublox => ublox_serialization}/serialization_ros.hpp (92%) diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp index 7c4eeb2d..b5375e89 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp @@ -31,7 +31,7 @@ #define UBLOX_SERIALIZATION_UBLOX_MSGS_HPP #include -#include +#include #include /// diff --git a/ublox_serialization/include/ublox/checksum.hpp b/ublox_serialization/include/ublox_serialization/checksum.hpp similarity index 94% rename from ublox_serialization/include/ublox/checksum.hpp rename to ublox_serialization/include/ublox_serialization/checksum.hpp index e35f4822..471882cb 100644 --- a/ublox_serialization/include/ublox/checksum.hpp +++ b/ublox_serialization/include/ublox_serialization/checksum.hpp @@ -26,10 +26,10 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_MSGS_CHECKSUM_HPP -#define UBLOX_MSGS_CHECKSUM_HPP +#ifndef UBLOX_SERIALIZATION_CHECKSUM_HPP +#define UBLOX_SERIALIZATION_CHECKSUM_HPP -#include +#include namespace ublox { @@ -67,6 +67,6 @@ static inline uint16_t calculateChecksum(const uint8_t *data, return checksum; } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_MSGS_CHECKSUM_HPP +#endif // UBLOX_SERIALIZATION_CHECKSUM_HPP diff --git a/ublox_serialization/include/ublox/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp similarity index 98% rename from ublox_serialization/include/ublox/serialization.hpp rename to ublox_serialization/include/ublox_serialization/serialization.hpp index 1ff4f701..8fa8beb1 100644 --- a/ublox_serialization/include/ublox/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -26,13 +26,15 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_SERIALIZATION_HPP -#define UBLOX_SERIALIZATION_HPP +#ifndef UBLOX_SERIALIZATION_SERIALIZATION_HPP +#define UBLOX_SERIALIZATION_SERIALIZATION_HPP -#include -#include -#include #include +#include +#include +#include + +#include #include "checksum.hpp" @@ -407,7 +409,9 @@ class Writer { size_ -= options_.header_length; // write message - if (message) std::copy(message, message + length, data_); + if (message) { + std::copy(message, message + length, data_); + } data_ += length; size_ -= length; @@ -435,7 +439,7 @@ class Writer { Options options_; }; -} // namespace ublox +} // namespace ublox // Use to declare u-blox messages and message serializers #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ @@ -457,4 +461,4 @@ class Writer { // use implementation of class Serializer in "serialization_ros.hpp" #include "serialization_ros.hpp" -#endif // UBLOX_SERIALIZATION_HPP +#endif // UBLOX_SERIALIZATION_SERIALIZATION_HPP diff --git a/ublox_serialization/include/ublox/serialization_ros.hpp b/ublox_serialization/include/ublox_serialization/serialization_ros.hpp similarity index 92% rename from ublox_serialization/include/ublox/serialization_ros.hpp rename to ublox_serialization/include/ublox_serialization/serialization_ros.hpp index 5eeca7fb..0a7b2bc9 100644 --- a/ublox_serialization/include/ublox/serialization_ros.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization_ros.hpp @@ -26,14 +26,16 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_SERIALIZATION_ROS_HPP -#define UBLOX_SERIALIZATION_ROS_HPP +#ifndef UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP +#define UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP -#include "serialization.hpp" -#include "checksum.hpp" +#include #include +#include "serialization.hpp" +#include "checksum.hpp" + namespace ublox { template @@ -54,6 +56,6 @@ void Serializer::write(uint8_t *data, uint32_t size, const T &message) { ros::serialization::Serializer::write(stream, message); } -} // namespace ublox +} // namespace ublox -#endif // UBLOX_SERIALIZATION_ROS_HPP +#endif // UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP From 3461552949c0a2e222f307b02ec22bd69fcac396 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 14:13:32 +0000 Subject: [PATCH 026/152] More rearrangement of header files to make a more sane structure. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 3 ++- ublox_gps/include/ublox_gps/gps.hpp | 2 +- .../ublox_msgs.hpp => ublox_msgs/serialization.hpp} | 10 ++++++---- ublox_msgs/src/ublox_msgs.cpp | 3 ++- 4 files changed, 11 insertions(+), 7 deletions(-) rename ublox_msgs/include/{ublox/serialization/ublox_msgs.hpp => ublox_msgs/serialization.hpp} (99%) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 5956954a..c80aea13 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -37,7 +37,8 @@ #include #include -#include + +#include namespace ublox_gps { diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 8df76585..7e8b8f8e 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -39,7 +39,7 @@ // ROS #include // Other u-blox packages -#include +#include // u-blox gps #include #include diff --git a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp similarity index 99% rename from ublox_msgs/include/ublox/serialization/ublox_msgs.hpp rename to ublox_msgs/include/ublox_msgs/serialization.hpp index b5375e89..957dc8fa 100644 --- a/ublox_msgs/include/ublox/serialization/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -27,8 +27,10 @@ //============================================================================== -#ifndef UBLOX_SERIALIZATION_UBLOX_MSGS_HPP -#define UBLOX_SERIALIZATION_UBLOX_MSGS_HPP +#ifndef UBLOX_MSGS_SERIALIZATION_HPP +#define UBLOX_MSGS_SERIALIZATION_HPP + +#include #include #include @@ -857,6 +859,6 @@ struct Serializer > { }; -} // namespace ublox +} // namespace ublox -#endif // UBLOX_SERIALIZATION_UBLOX_MSGS_HPP +#endif // UBLOX_MSGS_SERIALIZATION_HPP diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index bcc60217..c83153c8 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -26,9 +26,10 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== +#include #include -#include +#include template std::vector > ublox::Message::keys_; From 5b8fe74983203fbc02a31db4eda28fa1c97a97c2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 17:41:27 +0000 Subject: [PATCH 027/152] Move serialization into the ublox_serialization module. While using the ROS serialization was clever, it doesn't work with ROS 2, for instance. Move the serialization code into the serialization module. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 1 - ublox_gps/src/node.cpp | 2 +- .../include/ublox_msgs/serialization.hpp | 2946 ++++++++++++++--- .../ublox_serialization/serialization.hpp | 396 ++- .../ublox_serialization/serialization_ros.hpp | 61 - 5 files changed, 2834 insertions(+), 572 deletions(-) delete mode 100644 ublox_serialization/include/ublox_serialization/serialization_ros.hpp diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 37395adb..3d24aae9 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -37,7 +37,6 @@ // ROS includes #include #include -#include #include #include // ROS messages diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 3e00d8ef..6af317ae 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -411,7 +411,7 @@ void UbloxNode::processMonVer() { } ROS_DEBUG("%s, HW VER: %s", monVer.sw_version.c_array(), - monVer.hw_version.c_array()); + monVer.hw_version.c_array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 957dc8fa..3dc0bdd0 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -44,15 +44,210 @@ namespace ublox { +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::Ack_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cls_id); + stream.next(m.msg_id); + } + + inline static uint32_t serializedLength(const ublox_msgs::Ack_ &m) { + (void)m; + return 2; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::Ack_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cls_id); + stream.next(m.msg_id); + } +}; + +/// +/// @brief Serializes the AidALM message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::AidALM_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.week); + + m.dwrd.clear(); + if (count == 40) { + typename ublox_msgs::AidALM_::_dwrd_type::value_type temp; + m.dwrd.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp); + m.dwrd.push_back(temp); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::AidALM_ &m) { + return 8 + (4 * m.dwrd.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::AidALM_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.week); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + serialize(stream, m.dwrd[i]); + } + } +}; + +/// +/// @brief Serializes the AidEPH message which has a repeated block. +/// +template +struct UbloxSerializer > +{ + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::AidEPH_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.svid); + stream.next(m.how); + m.sf1d.clear(); + m.sf2d.clear(); + m.sf3d.clear(); + + if (count == 104) { + typename ublox_msgs::AidEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::AidEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::AidEPH_::_sf3d_type::value_type temp3; + m.sf1d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp1); + m.sf1d.push_back(temp1); + } + m.sf2d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp2); + m.sf2d.push_back(temp2); + } + m.sf3d.resize(8); + for (std::size_t i = 0; i < 8; ++i) { + stream.next(temp3); + m.sf3d.push_back(temp3); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::AidEPH_ &m) { + return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::AidEPH_ &m) { + UbloxOStream stream(data, size); + stream.next(m.svid); + stream.next(m.how); + for (std::size_t i = 0; i < m.sf1d.size(); ++i) { + serialize(stream, m.sf1d[i]); + } + for (std::size_t i = 0; i < m.sf2d.size(); ++i) { + serialize(stream, m.sf2d[i]); + } + for (std::size_t i = 0; i < m.sf3d.size(); ++i) { + serialize(stream, m.sf3d[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::AidHUI_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.health); + stream.next(m.utc_a0); + stream.next(m.utc_a1); + stream.next(m.utc_tow); + stream.next(m.utc_wnt); + stream.next(m.utc_dn); + stream.next(m.utc_lsf); + stream.next(m.utc_spare); + stream.next(m.klob_a0); + stream.next(m.klob_a1); + stream.next(m.klob_a2); + stream.next(m.klob_a3); + stream.next(m.klob_b0); + stream.next(m.klob_b1); + stream.next(m.klob_b2); + stream.next(m.klob_b3); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::AidHUI_ &m) { + (void)m; + return 72; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::AidHUI_ &m) { + UbloxOStream stream(data, size); + stream.next(m.health); + stream.next(m.utc_a0); + stream.next(m.utc_a1); + stream.next(m.utc_tow); + stream.next(m.utc_wnt); + stream.next(m.utc_dn); + stream.next(m.utc_lsf); + stream.next(m.utc_spare); + stream.next(m.klob_a0); + stream.next(m.klob_a1); + stream.next(m.klob_a2); + stream.next(m.klob_a3); + stream.next(m.klob_b0); + stream.next(m.klob_b1); + stream.next(m.klob_b2); + stream.next(m.klob_b3); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgCFG_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.clear_mask); + stream.next(m.save_mask); + stream.next(m.load_mask); + stream.next(m.device_mask); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgCFG_ &m) { + (void)m; + return 13; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgCFG_ &m) { + UbloxOStream stream(data, size); + stream.next(m.clear_mask); + stream.next(m.save_mask); + stream.next(m.load_mask); + stream.next(m.device_mask); + } +}; + /// /// @brief Serializes the CfgDAT message which has a different length for /// get/set. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgDAT_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgDAT_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.datum_num); stream.next(m.datum_name); stream.next(m.maj_a); @@ -66,15 +261,16 @@ struct Serializer > { stream.next(m.scale); } - static uint32_t serializedLength(const ublox_msgs::CfgDAT_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::CfgDAT_ &m) { // this is the size of CfgDAT set messages // serializedLength is only used for writes so this is ok + (void)m; return 44; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgDAT_ &m) { - ros::serialization::OStream stream(data, size); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgDAT_ &m) { + UbloxOStream stream(data, size); // ignores datumNum & datumName stream.next(m.maj_a); stream.next(m.flat); @@ -88,465 +284,2036 @@ struct Serializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgDGNSS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.dgnss_mode); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgDGNSS_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgDGNSS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.dgnss_mode); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::CfgGNSSBlock_ & m) { + stream.next(m.gnss_id); + stream.next(m.res_trk_ch); + stream.next(m.max_trk_ch); + stream.next(m.reserved1); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgGNSSBlock_ & m) { + (void)m; + return 8; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::CfgGNSSBlock_ & m) { + stream.next(m.gnss_id); + stream.next(m.res_trk_ch); + stream.next(m.max_trk_ch); + stream.next(m.reserved1); + stream.next(m.flags); + } +}; + /// /// @brief Serializes the CfgGNSS message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgGNSS_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgGNSS_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.msg_ver); stream.next(m.num_trk_ch_hw); stream.next(m.num_trk_ch_use); stream.next(m.num_config_blocks); m.blocks.resize(m.num_config_blocks); for (std::size_t i = 0; i < m.blocks.size(); ++i) { - ros::serialization::deserialize(stream, m.blocks[i]); + deserialize(stream, m.blocks[i]); } } - static uint32_t serializedLength(const ublox_msgs::CfgGNSS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::CfgGNSS_ &m) { return 4 + 8 * m.num_config_blocks; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgGNSS_ &m) { + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgGNSS_ &m) { if (m.blocks.size() != m.num_config_blocks) { ROS_ERROR("CfgGNSS num_config_blocks must equal blocks size"); } - ros::serialization::OStream stream(data, size); + UbloxOStream stream(data, size); stream.next(m.msg_ver); stream.next(m.num_trk_ch_hw); stream.next(m.num_trk_ch_use); stream.next( static_cast::_num_config_blocks_type>(m.blocks.size())); for (std::size_t i = 0; i < m.blocks.size(); ++i) { - ros::serialization::serialize(stream, m.blocks[i]); + serialize(stream, m.blocks[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::CfgINFBlock_ & m) { + stream.next(m.protocol_id); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.inf_msg_mask[0]); + stream.next(m.inf_msg_mask[1]); + stream.next(m.inf_msg_mask[2]); + stream.next(m.inf_msg_mask[3]); + stream.next(m.inf_msg_mask[4]); + stream.next(m.inf_msg_mask[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgINFBlock_ & m) { + (void)m; + return 10; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::CfgINFBlock_ & m) { + stream.next(m.protocol_id); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.inf_msg_mask[0]); + stream.next(m.inf_msg_mask[1]); + stream.next(m.inf_msg_mask[2]); + stream.next(m.inf_msg_mask[3]); + stream.next(m.inf_msg_mask[4]); + stream.next(m.inf_msg_mask[5]); + } +}; + /// /// @brief Serializes the CfgInf message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgINF_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgINF_ &m) { + UbloxIStream stream(const_cast(data), count); int num_blocks = count / 10; m.blocks.resize(num_blocks); for (std::size_t i = 0; i < num_blocks; ++i) { - ros::serialization::deserialize(stream, m.blocks[i]); + deserialize(stream, m.blocks[i]); } } - static uint32_t serializedLength(const ublox_msgs::CfgINF_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::CfgINF_ &m) { return 10 * m.blocks.size(); } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgINF_ &m) { - ros::serialization::OStream stream(data, size); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgINF_ &m) { + UbloxOStream stream(data, size); for (std::size_t i = 0; i < m.blocks.size(); ++i) { - ros::serialization::serialize(stream, m.blocks[i]); + serialize(stream, m.blocks[i]); } } }; -/// -/// @brief Serializes the Inf message which has a dynamic length string. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::Inf_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - m.str.resize(count); - for (int i = 0; i < count; ++i) { - ros::serialization::deserialize(stream, m.str[i]); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgMSG_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rate); } - static uint32_t serializedLength(const ublox_msgs::Inf_ &m) { - return m.str.size(); + inline static uint32_t serializedLength(const ublox_msgs::CfgMSG_ &m) { + (void)m; + return 3; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::Inf_ &m) { - ros::serialization::OStream stream(data, size); - for (std::size_t i = 0; i < m.str.size(); ++i) { - ros::serialization::serialize(stream, m.str[i]); - } + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgMSG_ &m) { + UbloxOStream stream(data, size); + stream.next(m.msg_class); + stream.next(m.msg_id); + stream.next(m.rate); } }; -/// -/// @brief Serializes the MonVER message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::MonVER_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.sw_version); - stream.next(m.hw_version); - - m.extension.clear(); - int N = (count - 40) / 30; - m.extension.reserve(N); - typename ublox_msgs::MonVER_::_extension_type::value_type ext; - for (int i = 0; i < N; i++) { - // Read each extension string - stream.next(ext); - m.extension.push_back(ext); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgNAV5_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.mask); + stream.next(m.dyn_model); + stream.next(m.fix_mode); + stream.next(m.fixed_alt); + stream.next(m.fixed_alt_var); + stream.next(m.min_elev); + stream.next(m.dr_limit); + stream.next(m.p_dop); + stream.next(m.t_dop); + stream.next(m.p_acc); + stream.next(m.t_acc); + stream.next(m.static_hold_thresh); + stream.next(m.dgnss_time_out); + stream.next(m.cno_thresh_num_svs); + stream.next(m.cno_thresh); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.static_hold_max_dist); + stream.next(m.utc_standard); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.reserved2[4]); } - static uint32_t serializedLength(const ublox_msgs::MonVER_ &m) { - return 40 + (30 * m.extension.size()); + inline static uint32_t serializedLength(const ublox_msgs::CfgNAV5_ &m) { + (void)m; + return 36; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::MonVER_ &m) { - ros::serialization::OStream stream(data, size); - stream.next(m.sw_version); - stream.next(m.hw_version); - for (std::size_t i = 0; i < m.extension.size(); ++i) { - ros::serialization::serialize(stream, m.extension[i]); - } + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgNAV5_ &m) { + UbloxOStream stream(data, size); + stream.next(m.mask); + stream.next(m.dyn_model); + stream.next(m.fix_mode); + stream.next(m.fixed_alt); + stream.next(m.fixed_alt_var); + stream.next(m.min_elev); + stream.next(m.dr_limit); + stream.next(m.p_dop); + stream.next(m.t_dop); + stream.next(m.p_acc); + stream.next(m.t_acc); + stream.next(m.static_hold_thresh); + stream.next(m.dgnss_time_out); + stream.next(m.cno_thresh_num_svs); + stream.next(m.cno_thresh); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.static_hold_max_dist); + stream.next(m.utc_standard); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.reserved2[4]); } }; -/// -/// @brief Serializes the NavDGPS message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavDGPS_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.i_tow); - stream.next(m.age); - stream.next(m.base_id); - stream.next(m.base_health); - stream.next(m.num_ch); - stream.next(m.status); - stream.next(m.reserved1); - m.sv.resize(m.num_ch); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgNAVX5_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.mask1); + stream.next(m.mask2); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.min_svs); + stream.next(m.max_svs); + stream.next(m.min_cno); + stream.next(m.reserved2); + stream.next(m.ini_fix3d); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.ack_aiding); + stream.next(m.wkn_rollover); + stream.next(m.sig_atten_comp_mode); + stream.next(m.reserved4[0]); + stream.next(m.reserved4[1]); + stream.next(m.reserved4[2]); + stream.next(m.reserved4[3]); + stream.next(m.reserved4[4]); + stream.next(m.use_ppp); + stream.next(m.aop_cfg); + stream.next(m.reserved5[0]); + stream.next(m.reserved5[1]); + stream.next(m.aop_orb_max_err); + stream.next(m.reserved6[0]); + stream.next(m.reserved6[1]); + stream.next(m.reserved6[2]); + stream.next(m.reserved6[3]); + stream.next(m.reserved6[4]); + stream.next(m.reserved6[5]); + stream.next(m.reserved6[6]); + stream.next(m.use_adr); } - static uint32_t serializedLength(const ublox_msgs::NavDGPS_ &m) { - return 16 + 12 * m.num_ch; + inline static uint32_t serializedLength(const ublox_msgs::CfgNAVX5_ &m) { + (void)m; + return 40; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavDGPS_ &m) { - if (m.sv.size() != m.num_ch) { - ROS_ERROR("NavDGPS numCh must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.i_tow); - stream.next(m.age); - stream.next(m.base_id); - stream.next(m.base_health); - stream.next(static_cast::_num_ch_type>(m.sv.size())); - stream.next(m.status); - stream.next(m.reserved1); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgNAVX5_ &m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.mask1); + stream.next(m.mask2); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.min_svs); + stream.next(m.max_svs); + stream.next(m.min_cno); + stream.next(m.reserved2); + stream.next(m.ini_fix3d); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.ack_aiding); + stream.next(m.wkn_rollover); + stream.next(m.sig_atten_comp_mode); + stream.next(m.reserved4[0]); + stream.next(m.reserved4[1]); + stream.next(m.reserved4[2]); + stream.next(m.reserved4[3]); + stream.next(m.reserved4[4]); + stream.next(m.use_ppp); + stream.next(m.aop_cfg); + stream.next(m.reserved5[0]); + stream.next(m.reserved5[1]); + stream.next(m.aop_orb_max_err); + stream.next(m.reserved6[0]); + stream.next(m.reserved6[1]); + stream.next(m.reserved6[2]); + stream.next(m.reserved6[3]); + stream.next(m.reserved6[4]); + stream.next(m.reserved6[5]); + stream.next(m.reserved6[6]); + stream.next(m.use_adr); } }; - -/// -/// @brief Serializes the NavSBAS message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSBAS_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.i_tow); - stream.next(m.geo); - stream.next(m.mode); - stream.next(m.sys); - stream.next(m.service); - stream.next(m.cnt); - stream.next(m.reserved0); - m.sv.resize(m.cnt); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgNMEA_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.version); + stream.next(m.bds_talker_id[0]); + stream.next(m.bds_talker_id[1]); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); } - static uint32_t serializedLength(const ublox_msgs::NavSBAS_ &m) { - return 12 + 12 * m.cnt; + inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA_ &m) { + (void)m; + return 20; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSBAS_ &m) { - if (m.sv.size() != m.cnt) { - ROS_ERROR("NavSBAS cnt must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.i_tow); - stream.next(m.geo); - stream.next(m.mode); - stream.next(m.sys); - stream.next(m.service); - stream.next(static_cast::_cnt_type>(m.sv.size())); - stream.next(m.reserved0); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgNMEA_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.version); + stream.next(m.bds_talker_id[0]); + stream.next(m.bds_talker_id[1]); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); } }; -/// -/// @brief Serializes the NavSAT message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSAT_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.i_tow); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgNMEA6_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); stream.next(m.version); - stream.next(m.num_svs); - stream.next(m.reserved0); - m.sv.resize(m.num_svs); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); - } + stream.next(m.num_sv); + stream.next(m.flags); } - static uint32_t serializedLength(const ublox_msgs::NavSAT_ &m) { - return 8 + 12 * m.num_svs; + inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA6_ &m) { + (void)m; + return 4; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSAT_ &m) { - if (m.sv.size() != m.num_svs) { - ROS_ERROR("NavSAT num_svs must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.i_tow); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgNMEA6_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); stream.next(m.version); - stream.next(static_cast::_num_svs_type>(m.sv.size())); - stream.next(m.reserved0); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + stream.next(m.num_sv); + stream.next(m.flags); } }; -/// -/// @brief Serializes the NavDGPS message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSVINFO_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.i_tow); - stream.next(m.num_ch); - stream.next(m.global_flags); - stream.next(m.reserved2); - m.sv.resize(m.num_ch); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgNMEA7_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.reserved); } - static uint32_t serializedLength(const ublox_msgs::NavSVINFO_ &m) { - return 8 + 12 * m.num_ch; + inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA7_ &m) { + (void)m; + return 12; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSVINFO_ &m) { - if (m.sv.size() != m.num_ch) { - ROS_ERROR("NavSVINFO num_ch must equal sv size"); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgNMEA7_ &m) { + UbloxOStream stream(data, size); + stream.next(m.filter); + stream.next(m.nmea_version); + stream.next(m.num_sv); + stream.next(m.flags); + stream.next(m.gnss_to_filter); + stream.next(m.sv_numbering); + stream.next(m.main_talker_id); + stream.next(m.gsv_talker_id); + stream.next(m.reserved); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgPRT_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.port_id); + stream.next(m.reserved0); + stream.next(m.tx_ready); + stream.next(m.mode); + stream.next(m.baud_rate); + stream.next(m.in_proto_mask); + stream.next(m.out_proto_mask); + stream.next(m.flags); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgPRT_ &m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgPRT_ &m) { + UbloxOStream stream(data, size); + stream.next(m.port_id); + stream.next(m.reserved0); + stream.next(m.tx_ready); + stream.next(m.mode); + stream.next(m.baud_rate); + stream.next(m.in_proto_mask); + stream.next(m.out_proto_mask); + stream.next(m.flags); + stream.next(m.reserved1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgRATE_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.meas_rate); + stream.next(m.nav_rate); + stream.next(m.time_ref); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgRATE_ &m) { + (void)m; + return 6; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgRATE_ &m) { + UbloxOStream stream(data, size); + stream.next(m.meas_rate); + stream.next(m.nav_rate); + stream.next(m.time_ref); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgRST_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.nav_bbr_mask); + stream.next(m.reset_mode); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgRST_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgRST_ &m) { + UbloxOStream stream(data, size); + stream.next(m.nav_bbr_mask); + stream.next(m.reset_mode); + stream.next(m.reserved1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgSBAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.mode); + stream.next(m.usage); + stream.next(m.max_sbas); + stream.next(m.scanmode2); + stream.next(m.scanmode1); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgSBAS_ &m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgSBAS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.mode); + stream.next(m.usage); + stream.next(m.max_sbas); + stream.next(m.scanmode2); + stream.next(m.scanmode1); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::CfgTMODE3_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + stream.next(m.ecef_x_or_lat); + stream.next(m.ecef_y_or_lon); + stream.next(m.ecef_z_or_alt); + stream.next(m.ecef_x_or_lat_hp); + stream.next(m.ecef_y_or_lon_hp); + stream.next(m.ecef_z_or_alt_hp); + stream.next(m.reserved2); + stream.next(m.fixed_pos_acc); + stream.next(m.svin_min_dur); + stream.next(m.svin_acc_limit); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.reserved3[4]); + stream.next(m.reserved3[5]); + stream.next(m.reserved3[6]); + stream.next(m.reserved3[7]); + } + + inline static uint32_t serializedLength(const ublox_msgs::CfgTMODE3_ &m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::CfgTMODE3_ &m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + stream.next(m.ecef_x_or_lat); + stream.next(m.ecef_y_or_lon); + stream.next(m.ecef_z_or_alt); + stream.next(m.ecef_x_or_lat_hp); + stream.next(m.ecef_y_or_lon_hp); + stream.next(m.ecef_z_or_alt_hp); + stream.next(m.reserved2); + stream.next(m.fixed_pos_acc); + stream.next(m.svin_min_dur); + stream.next(m.svin_acc_limit); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.reserved3[4]); + stream.next(m.reserved3[5]); + stream.next(m.reserved3[6]); + stream.next(m.reserved3[7]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::EsfINS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.bitfield0); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.i_tow); + stream.next(m.x_ang_rate); + stream.next(m.y_ang_rate); + stream.next(m.z_ang_rate); + stream.next(m.x_accel); + stream.next(m.y_accel); + stream.next(m.z_accel); + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfINS_ & m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::EsfINS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.bitfield0); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.i_tow); + stream.next(m.x_ang_rate); + stream.next(m.y_ang_rate); + stream.next(m.z_ang_rate); + stream.next(m.x_accel); + stream.next(m.y_accel); + stream.next(m.z_accel); + } +}; + +/// +/// @brief Serializes the EsfMEAS message which has a repeated block and an +/// optional block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::EsfMEAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.time_tag); + stream.next(m.flags); + stream.next(m.id); + + bool calib_valid = m.flags & m.FLAGS_CALIB_T_TAG_VALID; + int data_size = (count - (calib_valid ? 12 : 8)) / 4; + // Repeating block + m.data.resize(data_size); + for (std::size_t i = 0; i < data_size; ++i) { + deserialize(stream, m.data[i]); + } + // Optional block + if (calib_valid) { + m.calib_t_tag.resize(1); + deserialize(stream, m.calib_t_tag[0]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfMEAS_ &m) { + return 4 + 8 * m.data.size() + 4 * m.calib_t_tag.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::EsfMEAS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.time_tag); + stream.next(m.flags); + stream.next(m.id); + for (std::size_t i = 0; i < m.data.size(); ++i) { + serialize(stream, m.data[i]); + } + for (std::size_t i = 0; i < m.calib_t_tag.size(); ++i) { + serialize(stream, m.calib_t_tag[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::EsfRAWBlock_ & m) { + stream.next(m.data); + stream.next(m.s_t_tag); + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfRAWBlock_ & m) { + (void)m; + return 8; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::EsfRAWBlock_ & m) { + stream.next(m.data); + stream.next(m.s_t_tag); + } +}; + +/// +/// @brief Serializes the EsfRAW message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::EsfRAW_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.reserved0); + m.blocks.clear(); + int num_blocks = (count - 4) / 8; + m.blocks.resize(num_blocks); + for (std::size_t i = 0; i < num_blocks; ++i) { + deserialize(stream, m.blocks[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfRAW_ &m) { + return 4 + 8 * m.blocks.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::EsfRAW_ &m) { + UbloxOStream stream(data, size); + stream.next(m.reserved0); + for (std::size_t i = 0; i < m.blocks.size(); ++i) { + serialize(stream, m.blocks[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::EsfSTATUSSens_ & m) { + stream.next(m.sens_status1); + stream.next(m.sens_status2); + stream.next(m.freq); + stream.next(m.faults); + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfSTATUSSens_ & m) { + (void)m; + return 4; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::EsfSTATUSSens_ & m) { + stream.next(m.sens_status1); + stream.next(m.sens_status2); + stream.next(m.freq); + stream.next(m.faults); + } +}; + +/// +/// @brief Serializes the EsfSTATUS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::EsfSTATUS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.fusion_mode); + stream.next(m.reserved2); + stream.next(m.num_sens); + m.sens.resize(m.num_sens); + for (std::size_t i = 0; i < m.sens.size(); ++i) { + deserialize(stream, m.sens[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::EsfSTATUS_ &m) { + return 16 + 4 * m.num_sens; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::EsfSTATUS_ &m) { + if (m.sens.size() != m.num_sens) { + ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.fusion_mode); + stream.next(m.reserved2); + stream.next(static_cast::_num_sens_type>(m.sens.size())); + for (std::size_t i = 0; i < m.sens.size(); ++i) { + serialize(stream, m.sens[i]); } - ros::serialization::OStream stream(data, size); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::HnrPVT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.nano); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.g_speed); + stream.next(m.speed); + stream.next(m.head_mot); + stream.next(m.head_veh); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + } + + inline static uint32_t serializedLength(const ublox_msgs::HnrPVT_ & m) { + (void)m; + return 72; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::HnrPVT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.nano); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.g_speed); + stream.next(m.speed); + stream.next(m.head_mot); + stream.next(m.head_veh); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + } +}; + +/// +/// @brief Serializes the Inf message which has a dynamic length string. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::Inf_ &m) { + UbloxIStream stream(const_cast(data), count); + m.str.resize(count); + for (int i = 0; i < count; ++i) { + deserialize(stream, m.str[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::Inf_ &m) { + return m.str.size(); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::Inf_ &m) { + UbloxOStream stream(data, size); + for (std::size_t i = 0; i < m.str.size(); ++i) { + serialize(stream, m.str[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::MonHW_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } + + inline static uint32_t serializedLength(const ublox_msgs::MonHW_ & m) { + (void)m; + return 60; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::MonHW_ & m) { + UbloxOStream stream(data, size); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::MonHW6_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.vp[17]); + stream.next(m.vp[18]); + stream.next(m.vp[19]); + stream.next(m.vp[20]); + stream.next(m.vp[21]); + stream.next(m.vp[22]); + stream.next(m.vp[23]); + stream.next(m.vp[24]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } + + inline static uint32_t serializedLength(const ublox_msgs::MonHW6_ & m) { + (void)m; + return 68; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::MonHW6_ & m) { + UbloxOStream stream(data, size); + stream.next(m.pin_sel); + stream.next(m.pin_bank); + stream.next(m.pin_dir); + stream.next(m.pin_val); + stream.next(m.noise_per_ms); + stream.next(m.agc_cnt); + stream.next(m.a_status); + stream.next(m.a_power); + stream.next(m.flags); + stream.next(m.reserved0); + stream.next(m.used_mask); + stream.next(m.vp[0]); + stream.next(m.vp[1]); + stream.next(m.vp[2]); + stream.next(m.vp[3]); + stream.next(m.vp[4]); + stream.next(m.vp[5]); + stream.next(m.vp[6]); + stream.next(m.vp[7]); + stream.next(m.vp[8]); + stream.next(m.vp[9]); + stream.next(m.vp[10]); + stream.next(m.vp[11]); + stream.next(m.vp[12]); + stream.next(m.vp[13]); + stream.next(m.vp[14]); + stream.next(m.vp[15]); + stream.next(m.vp[16]); + stream.next(m.vp[17]); + stream.next(m.vp[18]); + stream.next(m.vp[19]); + stream.next(m.vp[20]); + stream.next(m.vp[21]); + stream.next(m.vp[22]); + stream.next(m.vp[23]); + stream.next(m.vp[24]); + stream.next(m.jam_ind); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.pin_irq); + stream.next(m.pull_h); + stream.next(m.pull_l); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::MonVERExtension_ & m) { + stream.next(m.field); + } + + inline static uint32_t serializedLength(const ublox_msgs::MonVERExtension_ & m) { + (void)m; + return 30; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::MonVERExtension_ & m) { + stream.next(m.field); + } +}; + +/// +/// @brief Serializes the MonVER message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::MonVER_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.sw_version); + stream.next(m.hw_version); + + m.extension.clear(); + int N = (count - 40) / 30; + m.extension.resize(N); + typename ublox_msgs::MonVER_::_extension_type::value_type ext; + for (int i = 0; i < N; i++) { + // Read each extension string + deserialize(stream, m.extension[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::MonVER_ &m) { + return 40 + (30 * m.extension.size()); + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::MonVER_ &m) { + UbloxOStream stream(data, size); + stream.next(m.sw_version); + stream.next(m.hw_version); + for (std::size_t i = 0; i < m.extension.size(); ++i) { + serialize(stream, m.extension[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavATT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.roll); + stream.next(m.pitch); + stream.next(m.heading); + stream.next(m.acc_roll); + stream.next(m.acc_pitch); + stream.next(m.acc_heading); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavATT_ & m) { + (void)m; + return 32; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavATT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.roll); + stream.next(m.pitch); + stream.next(m.heading); + stream.next(m.acc_roll); + stream.next(m.acc_pitch); + stream.next(m.acc_heading); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavCLOCK_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.clk_b); + stream.next(m.clk_d); + stream.next(m.t_acc); + stream.next(m.f_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavCLOCK_ & m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavCLOCK_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.clk_b); + stream.next(m.clk_d); + stream.next(m.t_acc); + stream.next(m.f_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::NavDGPSSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.age_c); + stream.next(m.prc); + stream.next(m.prrc); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavDGPSSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::NavDGPSSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.age_c); + stream.next(m.prc); + stream.next(m.prrc); + } +}; + +/// +/// @brief Serializes the NavDGPS message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavDGPS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.age); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(m.num_ch); + stream.next(m.status); + stream.next(m.reserved1); + m.sv.resize(m.num_ch); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::NavDGPS_ &m) { + return 16 + 12 * m.num_ch; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavDGPS_ &m) { + if (m.sv.size() != m.num_ch) { + ROS_ERROR("NavDGPS numCh must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.age); + stream.next(m.base_id); + stream.next(m.base_health); + stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(m.status); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavPOSECEF_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavPOSECEF_ & m) { + (void)m; + return 20; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavPOSECEF_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavPOSLLH_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavPOSLLH_ & m) { + (void)m; + return 28; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavPOSLLH_ & m) { + UbloxOStream stream(data, size); stream.next(m.i_tow); - stream.next(static_cast::_num_ch_type>(m.sv.size())); - stream.next(m.global_flags); - stream.next(m.reserved2); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); } }; -/// -/// @brief Serializes the RxmRAW message which has a repeated block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmRAW_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcv_tow); - stream.next(m.week); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavPVT_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + stream.next(m.head_veh); + stream.next(m.mag_dec); + stream.next(m.mag_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavPVT_ & m) { + (void)m; + return 92; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavPVT_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + stream.next(m.head_veh); + stream.next(m.mag_dec); + stream.next(m.mag_acc); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavPVT7_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); + stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavPVT7_ & m) { + (void)m; + return 84; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavPVT7_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.min); + stream.next(m.sec); + stream.next(m.valid); + stream.next(m.t_acc); + stream.next(m.nano); + stream.next(m.fix_type); + stream.next(m.flags); + stream.next(m.flags2); stream.next(m.num_sv); + stream.next(m.lon); + stream.next(m.lat); + stream.next(m.height); + stream.next(m.h_msl); + stream.next(m.h_acc); + stream.next(m.v_acc); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.head_acc); + stream.next(m.p_dop); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + stream.next(m.reserved1[3]); + stream.next(m.reserved1[4]); + stream.next(m.reserved1[5]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavRELPOSNED_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); stream.next(m.reserved1); - m.sv.resize(m.num_sv); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); - } + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.flags); } - static uint32_t serializedLength(const ublox_msgs::RxmRAW_ &m) { - return 8 + 24 * m.num_sv; + inline static uint32_t serializedLength(const ublox_msgs::NavRELPOSNED_ & m) { + (void)m; + return 40; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmRAW_ &m) { - if (m.sv.size() != m.num_sv) { - ROS_ERROR("RxmRAW num_sv must equal sv size"); - } - ros::serialization::OStream stream(data, size); - stream.next(m.rcv_tow); - stream.next(m.week); - stream.next(static_cast::_num_sv_type>(m.sv.size())); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavRELPOSNED_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); stream.next(m.reserved1); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavRELPOSNED9_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_length); + stream.next(m.rel_pos_heading); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.rel_pos_hp_length); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.acc_length); + stream.next(m.acc_heading); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavRELPOSNED9_ & m) { + (void)m; + return 64; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavRELPOSNED9_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.ref_station_id); + stream.next(m.i_tow); + stream.next(m.rel_pos_n); + stream.next(m.rel_pos_e); + stream.next(m.rel_pos_d); + stream.next(m.rel_pos_length); + stream.next(m.rel_pos_heading); + stream.next(m.reserved2[0]); + stream.next(m.reserved2[1]); + stream.next(m.reserved2[2]); + stream.next(m.reserved2[3]); + stream.next(m.rel_pos_hpn); + stream.next(m.rel_pos_hpe); + stream.next(m.rel_pos_hpd); + stream.next(m.rel_pos_hp_length); + stream.next(m.acc_n); + stream.next(m.acc_e); + stream.next(m.acc_d); + stream.next(m.acc_length); + stream.next(m.acc_heading); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + stream.next(m.reserved3[2]); + stream.next(m.reserved3[3]); + stream.next(m.flags); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::NavSATSV_ & m) { + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + stream.next(m.flags); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSATSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::NavSATSV_ & m) { + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + stream.next(m.flags); } }; /// -/// @brief Serializes the RxmRAWX message which has a repeated block. +/// @brief Serializes the NavSAT message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmRAWX_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.rcv_tow); - stream.next(m.week); - stream.next(m.leap_s); - stream.next(m.num_meas); - stream.next(m.rec_stat); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSAT_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); stream.next(m.version); - stream.next(m.reserved1); - m.meas.resize(m.num_meas); - for (std::size_t i = 0; i < m.meas.size(); ++i) { - ros::serialization::deserialize(stream, m.meas[i]); + stream.next(m.num_svs); + stream.next(m.reserved0); + m.sv.resize(m.num_svs); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); } } - static uint32_t serializedLength(const ublox_msgs::RxmRAWX_ &m) { - return 16 + 32 * m.num_meas; + inline static uint32_t serializedLength(const ublox_msgs::NavSAT_ &m) { + return 8 + 12 * m.num_svs; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmRAWX_ &m) { - if (m.meas.size() != m.num_meas) { - ROS_ERROR("RxmRAWX num_meas must equal meas size"); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSAT_ &m) { + if (m.sv.size() != m.num_svs) { + ROS_ERROR("NavSAT num_svs must equal sv size"); } - ros::serialization::OStream stream(data, size); - stream.next(m.rcv_tow); - stream.next(m.week); - stream.next(m.leap_s); - stream.next(static_cast::_num_meas_type>(m.meas.size())); - stream.next(m.rec_stat); + UbloxOStream stream(data, size); + stream.next(m.i_tow); stream.next(m.version); - stream.next(m.reserved1); - for (std::size_t i = 0; i < m.meas.size(); ++i) { - ros::serialization::serialize(stream, m.meas[i]); + stream.next(static_cast::_num_svs_type>(m.sv.size())); + stream.next(m.reserved0); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::NavSBASSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.udre); + stream.next(m.sv_sys); + stream.next(m.sv_service); + stream.next(m.reserved1); + stream.next(m.prc); + stream.next(m.reserved2); + stream.next(m.ic); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSBASSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::NavSBASSV_ & m) { + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.udre); + stream.next(m.sv_sys); + stream.next(m.sv_service); + stream.next(m.reserved1); + stream.next(m.prc); + stream.next(m.reserved2); + stream.next(m.ic); + } +}; + /// -/// @brief Serializes the RxmSFRBX message which has a repeated block. +/// @brief Serializes the NavSBAS message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmSFRBX_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.gnss_id); - stream.next(m.sv_id); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSBAS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.geo); + stream.next(m.mode); + stream.next(m.sys); + stream.next(m.service); + stream.next(m.cnt); stream.next(m.reserved0); - stream.next(m.freq_id); - stream.next(m.num_words); - stream.next(m.chn); - stream.next(m.version); - stream.next(m.reserved1); - m.dwrd.resize(m.num_words); - for (std::size_t i = 0; i < m.dwrd.size(); ++i) { - ros::serialization::deserialize(stream, m.dwrd[i]); + m.sv.resize(m.cnt); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); } } - static uint32_t serializedLength(const ublox_msgs::RxmSFRBX_ &m) { - return 8 + 4 * m.num_words; + inline static uint32_t serializedLength(const ublox_msgs::NavSBAS_ &m) { + return 12 + 12 * m.cnt; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmSFRBX_ &m) { - if (m.dwrd.size() != m.num_words) { - ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSBAS_ &m) { + if (m.sv.size() != m.cnt) { + ROS_ERROR("NavSBAS cnt must equal sv size"); } - ros::serialization::OStream stream(data, size); - stream.next(m.gnss_id); - stream.next(m.sv_id); + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.geo); + stream.next(m.mode); + stream.next(m.sys); + stream.next(m.service); + stream.next(static_cast::_cnt_type>(m.sv.size())); stream.next(m.reserved0); - stream.next(m.freq_id); - stream.next(static_cast::_num_words_type>(m.dwrd.size())); - stream.next(m.chn); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); + } + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSOL_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + stream.next(m.ecef_vx); + stream.next(m.ecef_vy); + stream.next(m.ecef_vz); + stream.next(m.s_acc); + stream.next(m.p_dop); + stream.next(m.reserved1); + stream.next(m.num_sv); + stream.next(m.reserved2); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSOL_ & m) { + (void)m; + return 52; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSOL_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.p_acc); + stream.next(m.ecef_vx); + stream.next(m.ecef_vy); + stream.next(m.ecef_vz); + stream.next(m.s_acc); + stream.next(m.p_dop); + stream.next(m.reserved1); + stream.next(m.num_sv); + stream.next(m.reserved2); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSTATUS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.fix_stat); + stream.next(m.flags2); + stream.next(m.ttff); + stream.next(m.msss); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSTATUS_ & m) { + (void)m; + return 16; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSTATUS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.gps_fix); + stream.next(m.flags); + stream.next(m.fix_stat); + stream.next(m.flags2); + stream.next(m.ttff); + stream.next(m.msss); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSVIN_ & m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.i_tow); + stream.next(m.dur); + stream.next(m.mean_x); + stream.next(m.mean_y); + stream.next(m.mean_z); + stream.next(m.mean_xhp); + stream.next(m.mean_yhp); + stream.next(m.mean_zhp); stream.next(m.reserved1); - for (std::size_t i = 0; i < m.dwrd.size(); ++i) { - ros::serialization::serialize(stream, m.dwrd[i]); - } + stream.next(m.mean_acc); + stream.next(m.obs); + stream.next(m.valid); + stream.next(m.active); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSVIN_ & m) { + (void)m; + return 40; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSVIN_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.i_tow); + stream.next(m.dur); + stream.next(m.mean_x); + stream.next(m.mean_y); + stream.next(m.mean_z); + stream.next(m.mean_xhp); + stream.next(m.mean_yhp); + stream.next(m.mean_zhp); + stream.next(m.reserved1); + stream.next(m.mean_acc); + stream.next(m.obs); + stream.next(m.valid); + stream.next(m.active); + stream.next(m.reserved3[0]); + stream.next(m.reserved3[1]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::NavSVINFOSV_ & m) { + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.quality); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSVINFOSV_ & m) { + (void)m; + return 12; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::NavSVINFOSV_ & m) { + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.flags); + stream.next(m.quality); + stream.next(m.cno); + stream.next(m.elev); + stream.next(m.azim); + stream.next(m.pr_res); } }; /// -/// @brief Serializes the RxmSVSI message which has a repeated block. +/// @brief Serializes the NavSVINFO message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmSVSI_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavSVINFO_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); - stream.next(m.week); - stream.next(m.num_vis); - stream.next(m.num_sv); - m.sv.resize(m.num_sv); + stream.next(m.num_ch); + stream.next(m.global_flags); + stream.next(m.reserved2); + m.sv.resize(m.num_ch); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::NavSVINFO_ &m) { + return 8 + 12 * m.num_ch; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavSVINFO_ &m) { + if (m.sv.size() != m.num_ch) { + ROS_ERROR("NavSVINFO num_ch must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(m.global_flags); + stream.next(m.reserved2); for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::deserialize(stream, m.sv[i]); + serialize(stream, m.sv[i]); } } +}; - static uint32_t serializedLength(const ublox_msgs::RxmSVSI_ &m) { - return 8 + 6 * m.num_sv; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::NavVELNED_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.speed); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.c_acc); } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmSVSI_ &m) { - if (m.sv.size() != m.num_sv) { - ROS_ERROR("RxmSVSI num_sv must equal sv size"); - } - ros::serialization::OStream stream(data, size); + inline static uint32_t serializedLength(const ublox_msgs::NavVELNED_ & m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::NavVELNED_ & m) { + UbloxOStream stream(data, size); stream.next(m.i_tow); - stream.next(m.week); - stream.next(m.num_vis); - stream.next(static_cast::_num_sv_type>(m.sv.size())); - for (std::size_t i = 0; i < m.sv.size(); ++i) { - ros::serialization::serialize(stream, m.sv[i]); - } + stream.next(m.vel_n); + stream.next(m.vel_e); + stream.next(m.vel_d); + stream.next(m.speed); + stream.next(m.g_speed); + stream.next(m.heading); + stream.next(m.s_acc); + stream.next(m.c_acc); } }; @@ -554,10 +2321,10 @@ struct Serializer > { /// @brief Serializes the RxmALM message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmALM_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmALM_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); @@ -572,17 +2339,17 @@ struct Serializer > { } } - static uint32_t serializedLength(const ublox_msgs::RxmALM_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::RxmALM_ &m) { return 8 + (4 * m.dwrd.size()); } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmALM_ &m) { - ros::serialization::OStream stream(data, size); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmALM_ &m) { + UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.week); for (std::size_t i = 0; i < m.dwrd.size(); ++i) { - ros::serialization::serialize(stream, m.dwrd[i]); + serialize(stream, m.dwrd[i]); } } }; @@ -591,11 +2358,11 @@ struct Serializer > { /// @brief Serializes the RxmEPH message which has a repeated block. /// template -struct Serializer > +struct UbloxSerializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmEPH_ &m) { - ros::serialization::IStream stream(const_cast(data), count); + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmEPH_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); m.sf1d.clear(); @@ -625,239 +2392,456 @@ struct Serializer > } } - static uint32_t serializedLength(const ublox_msgs::RxmEPH_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::RxmEPH_ &m) { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmEPH_ &m) { - ros::serialization::OStream stream(data, size); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmEPH_ &m) { + UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.how); for(std::size_t i = 0; i < m.sf1d.size(); ++i) { - ros::serialization::serialize(stream, m.sf1d[i]); + serialize(stream, m.sf1d[i]); } for(std::size_t i = 0; i < m.sf2d.size(); ++i) { - ros::serialization::serialize(stream, m.sf2d[i]); + serialize(stream, m.sf2d[i]); } for(std::size_t i = 0; i < m.sf3d.size(); ++i) { - ros::serialization::serialize(stream, m.sf3d[i]); + serialize(stream, m.sf3d[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::RxmRAWSV_ & m) { + stream.next(m.cp_mes); + stream.next(m.pr_mes); + stream.next(m.do_mes); + stream.next(m.sv); + stream.next(m.mes_qi); + stream.next(m.cno); + stream.next(m.lli); + } + + inline static uint32_t serializedLength(const ublox_msgs::RxmRAWSV_ & m) { + (void)m; + return 24; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::RxmRAWSV_ & m) { + stream.next(m.cp_mes); + stream.next(m.pr_mes); + stream.next(m.do_mes); + stream.next(m.sv); + stream.next(m.mes_qi); + stream.next(m.cno); + stream.next(m.lli); + } +}; + /// -/// @brief Serializes the AidALM message which has a repeated block. +/// @brief Serializes the RxmRAW message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::AidALM_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmRAW_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.rcv_tow); stream.next(m.week); - - m.dwrd.clear(); - if (count == 40) { - typename ublox_msgs::AidALM_::_dwrd_type::value_type temp; - m.dwrd.resize(8); - for (std::size_t i = 0; i < 8; ++i) { - stream.next(temp); - m.dwrd.push_back(temp); - } + stream.next(m.num_sv); + stream.next(m.reserved1); + m.sv.resize(m.num_sv); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); } } - static uint32_t serializedLength(const ublox_msgs::AidALM_ &m) { - return 8 + (4 * m.dwrd.size()); + inline static uint32_t serializedLength(const ublox_msgs::RxmRAW_ &m) { + return 8 + 24 * m.num_sv; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::AidALM_ &m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmRAW_ &m) { + if (m.sv.size() != m.num_sv) { + ROS_ERROR("RxmRAW num_sv must equal sv size"); + } + UbloxOStream stream(data, size); + stream.next(m.rcv_tow); stream.next(m.week); - for (std::size_t i = 0; i < m.dwrd.size(); ++i) { - ros::serialization::serialize(stream, m.dwrd[i]); + stream.next(static_cast::_num_sv_type>(m.sv.size())); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::RxmRAWXMeas_ & m) { + stream.next(m.pr_mes); + stream.next(m.cp_mes); + stream.next(m.do_mes); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(m.locktime); + stream.next(m.cno); + stream.next(m.pr_stdev); + stream.next(m.cp_stdev); + stream.next(m.do_stdev); + stream.next(m.trk_stat); + stream.next(m.reserved1); + } + + inline static uint32_t serializedLength(const ublox_msgs::RxmRAWXMeas_ & m) { + (void)m; + return 32; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::RxmRAWXMeas_ & m) { + stream.next(m.pr_mes); + stream.next(m.cp_mes); + stream.next(m.do_mes); + stream.next(m.gnss_id); + stream.next(m.sv_id); + stream.next(m.reserved0); + stream.next(m.freq_id); + stream.next(m.locktime); + stream.next(m.cno); + stream.next(m.pr_stdev); + stream.next(m.cp_stdev); + stream.next(m.do_stdev); + stream.next(m.trk_stat); + stream.next(m.reserved1); + } +}; + /// -/// @brief Serializes the AidEPH message which has a repeated block. +/// @brief Serializes the RxmRAWX message which has a repeated block. /// template -struct Serializer > -{ - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::AidEPH_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.svid); - stream.next(m.how); - m.sf1d.clear(); - m.sf2d.clear(); - m.sf3d.clear(); - - if (count == 104) { - typename ublox_msgs::AidEPH_::_sf1d_type::value_type temp1; - typename ublox_msgs::AidEPH_::_sf2d_type::value_type temp2; - typename ublox_msgs::AidEPH_::_sf3d_type::value_type temp3; - m.sf1d.resize(8); - for (std::size_t i = 0; i < 8; ++i) { - stream.next(temp1); - m.sf1d.push_back(temp1); - } - m.sf2d.resize(8); - for (std::size_t i = 0; i < 8; ++i) { - stream.next(temp2); - m.sf2d.push_back(temp2); - } - m.sf3d.resize(8); - for (std::size_t i = 0; i < 8; ++i) { - stream.next(temp3); - m.sf3d.push_back(temp3); - } +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmRAWX_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.num_meas); + stream.next(m.rec_stat); + stream.next(m.version); + stream.next(m.reserved1); + m.meas.resize(m.num_meas); + for (std::size_t i = 0; i < m.meas.size(); ++i) { + deserialize(stream, m.meas[i]); } } - static uint32_t serializedLength(const ublox_msgs::AidEPH_ &m) { - return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); + inline static uint32_t serializedLength(const ublox_msgs::RxmRAWX_ &m) { + return 16 + 32 * m.num_meas; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::AidEPH_ &m) { - ros::serialization::OStream stream(data, size); - stream.next(m.svid); - stream.next(m.how); - for (std::size_t i = 0; i < m.sf1d.size(); ++i) { - ros::serialization::serialize(stream, m.sf1d[i]); - } - for (std::size_t i = 0; i < m.sf2d.size(); ++i) { - ros::serialization::serialize(stream, m.sf2d[i]); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmRAWX_ &m) { + if (m.meas.size() != m.num_meas) { + ROS_ERROR("RxmRAWX num_meas must equal meas size"); } - for (std::size_t i = 0; i < m.sf3d.size(); ++i) { - ros::serialization::serialize(stream, m.sf3d[i]); + UbloxOStream stream(data, size); + stream.next(m.rcv_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(static_cast::_num_meas_type>(m.meas.size())); + stream.next(m.rec_stat); + stream.next(m.version); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.meas.size(); ++i) { + serialize(stream, m.meas[i]); } } }; -/// -/// @brief Serializes the EsfMEAS message which has a repeated block and an -/// optional block. -/// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfMEAS_ &m) { - ros::serialization::IStream stream(const_cast(data), count); - stream.next(m.time_tag); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmRTCM_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); stream.next(m.flags); - stream.next(m.id); - - bool calib_valid = m.flags & m.FLAGS_CALIB_T_TAG_VALID; - int data_size = (count - (calib_valid ? 12 : 8)) / 4; - // Repeating block - m.data.resize(data_size); - for (std::size_t i = 0; i < data_size; ++i) { - ros::serialization::deserialize(stream, m.data[i]); - } - // Optional block - if (calib_valid) { - m.calib_t_tag.resize(1); - ros::serialization::deserialize(stream, m.calib_t_tag[0]); - } + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.ref_station); + stream.next(m.msg_type); } - static uint32_t serializedLength(const ublox_msgs::EsfMEAS_ &m) { - return 4 + 8 * m.data.size() + 4 * m.calib_t_tag.size(); + inline static uint32_t serializedLength(const ublox_msgs::RxmRTCM_ & m) { + (void)m; + return 8; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfMEAS_ &m) { - ros::serialization::OStream stream(data, size); - stream.next(m.time_tag); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmRTCM_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); stream.next(m.flags); - stream.next(m.id); - for (std::size_t i = 0; i < m.data.size(); ++i) { - ros::serialization::serialize(stream, m.data[i]); - } - for (std::size_t i = 0; i < m.calib_t_tag.size(); ++i) { - ros::serialization::serialize(stream, m.calib_t_tag[i]); - } + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.ref_station); + stream.next(m.msg_type); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmSFRB_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.dwrd[0]); + stream.next(m.dwrd[1]); + stream.next(m.dwrd[2]); + stream.next(m.dwrd[3]); + stream.next(m.dwrd[4]); + stream.next(m.dwrd[5]); + stream.next(m.dwrd[6]); + stream.next(m.dwrd[7]); + stream.next(m.dwrd[8]); + stream.next(m.dwrd[9]); + } + + inline static uint32_t serializedLength(const ublox_msgs::RxmSFRB_ & m) { + (void)m; + return 42; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmSFRB_ & m) { + UbloxOStream stream(data, size); + stream.next(m.chn); + stream.next(m.svid); + stream.next(m.dwrd[0]); + stream.next(m.dwrd[1]); + stream.next(m.dwrd[2]); + stream.next(m.dwrd[3]); + stream.next(m.dwrd[4]); + stream.next(m.dwrd[5]); + stream.next(m.dwrd[6]); + stream.next(m.dwrd[7]); + stream.next(m.dwrd[8]); + stream.next(m.dwrd[9]); } }; /// -/// @brief Serializes the EsfRAW message which has a repeated block. +/// @brief Serializes the RxmSFRBX message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfRAW_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmSFRBX_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.gnss_id); + stream.next(m.sv_id); stream.next(m.reserved0); - m.blocks.clear(); - int num_blocks = (count - 4) / 8; - m.blocks.resize(num_blocks); - for (std::size_t i = 0; i < num_blocks; ++i) { - ros::serialization::deserialize(stream, m.blocks[i]); + stream.next(m.freq_id); + stream.next(m.num_words); + stream.next(m.chn); + stream.next(m.version); + stream.next(m.reserved1); + m.dwrd.resize(m.num_words); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + deserialize(stream, m.dwrd[i]); } } - static uint32_t serializedLength(const ublox_msgs::EsfRAW_ &m) { - return 4 + 8 * m.blocks.size(); + inline static uint32_t serializedLength(const ublox_msgs::RxmSFRBX_ &m) { + return 8 + 4 * m.num_words; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfRAW_ &m) { - ros::serialization::OStream stream(data, size); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmSFRBX_ &m) { + if (m.dwrd.size() != m.num_words) { + ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); + } + UbloxOStream stream(data, size); + stream.next(m.gnss_id); + stream.next(m.sv_id); stream.next(m.reserved0); - for (std::size_t i = 0; i < m.blocks.size(); ++i) { - ros::serialization::serialize(stream, m.blocks[i]); + stream.next(m.freq_id); + stream.next(static_cast::_num_words_type>(m.dwrd.size())); + stream.next(m.chn); + stream.next(m.version); + stream.next(m.reserved1); + for (std::size_t i = 0; i < m.dwrd.size(); ++i) { + serialize(stream, m.dwrd[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::RxmSVSISV_ & m) { + stream.next(m.svid); + stream.next(m.sv_flag); + stream.next(m.azim); + stream.next(m.elev); + stream.next(m.age); + } + + inline static uint32_t serializedLength(const ublox_msgs::RxmSVSISV_ & m) { + (void)m; + return 6; + } + + inline static void write(UbloxOStream& stream, const ublox_msgs::RxmSVSISV_ & m) { + stream.next(m.svid); + stream.next(m.sv_flag); + stream.next(m.azim); + stream.next(m.elev); + stream.next(m.age); + } +}; + /// -/// @brief Serializes the EsfSTATUS message which has a repeated block. +/// @brief Serializes the RxmSVSI message which has a repeated block. /// template -struct Serializer > { - static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfSTATUS_ &m) { - ros::serialization::IStream stream(const_cast(data), count); +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::RxmSVSI_ &m) { + UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); - stream.next(m.version); - stream.next(m.fusion_mode); - stream.next(m.reserved2); - stream.next(m.num_sens); - m.sens.resize(m.num_sens); - for (std::size_t i = 0; i < m.sens.size(); ++i) { - ros::serialization::deserialize(stream, m.sens[i]); + stream.next(m.week); + stream.next(m.num_vis); + stream.next(m.num_sv); + m.sv.resize(m.num_sv); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + deserialize(stream, m.sv[i]); } } - static uint32_t serializedLength(const ublox_msgs::EsfSTATUS_ &m) { - return 16 + 4 * m.num_sens; + inline static uint32_t serializedLength(const ublox_msgs::RxmSVSI_ &m) { + return 8 + 6 * m.num_sv; } - static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfSTATUS_ &m) { - if (m.sens.size() != m.num_sens) { - ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::RxmSVSI_ &m) { + if (m.sv.size() != m.num_sv) { + ROS_ERROR("RxmSVSI num_sv must equal sv size"); } - ros::serialization::OStream stream(data, size); + UbloxOStream stream(data, size); stream.next(m.i_tow); - stream.next(m.version); - stream.next(m.fusion_mode); - stream.next(m.reserved2); - stream.next(static_cast::_num_sens_type>(m.sens.size())); - for (std::size_t i = 0; i < m.sens.size(); ++i) { - ros::serialization::serialize(stream, m.sens[i]); + stream.next(m.week); + stream.next(m.num_vis); + stream.next(static_cast::_num_sv_type>(m.sv.size())); + for (std::size_t i = 0; i < m.sv.size(); ++i) { + serialize(stream, m.sv[i]); } } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::TimTM2_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.ch); + stream.next(m.flags); + stream.next(m.rising_edge_count); + stream.next(m.wn_r); + stream.next(m.wn_f); + stream.next(m.tow_ms_r); + stream.next(m.tow_sub_ms_r); + stream.next(m.tow_ms_f); + stream.next(m.tow_sub_ms_f); + stream.next(m.acc_est); + } + + inline static uint32_t serializedLength(const ublox_msgs::TimTM2_ &m) { + (void)m; + return 28; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::TimTM2_ &m) { + UbloxOStream stream(data, size); + stream.next(m.ch); + stream.next(m.flags); + stream.next(m.rising_edge_count); + stream.next(m.wn_r); + stream.next(m.wn_f); + stream.next(m.tow_ms_r); + stream.next(m.tow_sub_ms_r); + stream.next(m.tow_ms_f); + stream.next(m.tow_sub_ms_f); + stream.next(m.acc_est); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::UpdSOS_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cmd); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::UpdSOS_ &m) { + (void)m; + return 4; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::UpdSOS_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cmd); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } +}; + +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::UpdSOSAck_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.cmd); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.response); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } + + inline static uint32_t serializedLength(const ublox_msgs::UpdSOSAck_ &m) { + (void)m; + return 8; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::UpdSOSAck_ &m) { + UbloxOStream stream(data, size); + stream.next(m.cmd); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + stream.next(m.reserved0[2]); + stream.next(m.response); + stream.next(m.reserved1[0]); + stream.next(m.reserved1[1]); + stream.next(m.reserved1[2]); + } +}; } // namespace ublox diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 8fa8beb1..98592aae 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -36,10 +36,12 @@ #include +#include + #include "checksum.hpp" /// -/// This file defines the Serializer template class which encodes and decodes +/// This file defines the UbloxSerializer template class which encodes and decodes /// specific message types. /// The Reader class decodes messages and from a buffer and the Writer class /// encodes messages and writes them to a buffer. @@ -64,34 +66,376 @@ static const uint8_t kHeaderLength = 6; static const uint8_t kChecksumLength = 2; /** - * @brief Encodes and decodes messages. + * \brief Templated serialization class. Default implementation provides backwards compatibility with + * old message types. + * + * Specializing the UbloxSerializer class is the only thing you need to do to get the ROS serialization system + * to work with a type. */ +template +struct UbloxSerializer +{ + /** + * \brief Write an object to the stream. Normally the stream passed in here will be a UbloxOStream + */ + template + inline static void write(Stream& stream, const T & t); + + /** + * \brief Read an object from the stream. Normally the stream passed in here will be a UbloxIStream + */ + template + inline static void read(Stream& stream, T & t); + + /** + * \brief Determine the serialized length of an object. + */ + inline static uint32_t serializedLength(const T & t); +}; + template -struct Serializer { +struct UbloxSerializer::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value>::type> +{ + template + inline static void write(Stream& stream, const T v) { + *reinterpret_cast(stream.advance(sizeof(v))) = v; + } + + template + inline static void read(Stream& stream, T& v) { + v = *reinterpret_cast(stream.advance(sizeof(v))); + } + + inline static uint32_t serializedLength(const T&) { + return sizeof(T); + } +}; + +/** + * \brief Serialize an object. Stream here should normally be a UbloxOStream + */ +template +inline void serialize(Stream& stream, const T& t) { + UbloxSerializer::write(stream, t); +} + +/** + * \brief Deserialize an object. Stream here should normally be a UbloxIStream + */ +template +inline void deserialize(Stream& stream, T& t) { + UbloxSerializer::read(stream, t); +} + +/** + * \brief Determine the serialized length of an object + */ +template +inline uint32_t serializationLength(const T& t) { + return UbloxSerializer::serializedLength(t); +} + +/** + * \brief Vector serializer. Default implementation does nothing + */ +template +struct VectorUbloxSerializer +{}; + +/** + * \brief Vector serializer, specialized for fixed-size simple types + */ +template +struct VectorUbloxSerializer::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value>::type> +{ + typedef std::vector::other> VecType; + typedef typename VecType::iterator IteratorType; + typedef typename VecType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const VecType& v) { + uint32_t len = static_cast(v.size()); + stream.next(len); + if (!v.empty()) { + const uint32_t data_len = len * static_cast(sizeof(T)); + memcpy(stream.advance(data_len), &v.front(), data_len); + } + } + + template + inline static void read(Stream& stream, VecType& v) { + uint32_t len; + stream.next(len); + v.resize(len); + + if (len > 0) { + const uint32_t data_len = static_cast(sizeof(T)) * len; + memcpy(&v.front(), stream.advance(data_len), data_len); + } + } + + inline static uint32_t serializedLength(const VecType& v) { + return 4 + v.size() * static_cast(sizeof(T)); + } +}; + +/** + * \brief serialize version for std::vector + */ +template +inline void serialize(Stream& stream, const std::vector& t) { + VectorUbloxSerializer::write(stream, t); +} + +/** + * \brief deserialize version for std::vector + */ +template +inline void deserialize(Stream& stream, std::vector& t) { + VectorUbloxSerializer::read(stream, t); +} + +/** + * \brief serializationLength version for std::vector + */ +template +inline uint32_t serializationLength(const std::vector& t) { + return VectorUbloxSerializer::serializedLength(t); +} + +/** + * \brief Array serializer, default implementation does nothing + */ +template +struct StdArrayUbloxSerializer +{}; + +/** + * \brief Array serializer, specialized for fixed-size, simple types + */ +template +struct StdArrayUbloxSerializer::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value>::type> +{ + typedef std::array ArrayType; + typedef typename ArrayType::iterator IteratorType; + typedef typename ArrayType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(stream.advance(data_len), &v.front(), data_len); + } + + template + inline static void read(Stream& stream, ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(&v.front(), stream.advance(data_len), data_len); + } + + inline static uint32_t serializedLength(const ArrayType& v) { + return N * sizeof(T); + } +}; + +/** + * \brief serialize version for std::array + */ +template +inline void serialize(Stream& stream, const std::array& t) +{ + StdArrayUbloxSerializer::write(stream, t); +} + +/** + * \brief deserialize version for std::array + */ +template +inline void deserialize(Stream& stream, std::array& t) { + StdArrayUbloxSerializer::read(stream, t); +} + +/** + * \brief serializationLength version for std::array + */ +template +inline uint32_t serializationLength(const std::array& t) +{ + return StdArrayUbloxSerializer::serializedLength(t); +} + +/** + * \brief Array serializer, default implementation does nothing + */ +template +struct BoostArraySerializer +{}; + +/** + * \brief Array serializer, specialized for fixed-size, simple types + */ +template +struct BoostArraySerializer::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value>::type> +{ + typedef boost::array ArrayType; + typedef typename ArrayType::iterator IteratorType; + typedef typename ArrayType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(stream.advance(data_len), &v.front(), data_len); + } + + template + inline static void read(Stream& stream, ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + std::memcpy(&v.front(), stream.advance(data_len), data_len); + } + + inline static uint32_t serializedLength(const ArrayType& v) { + return N * sizeof(T); + } +}; + +/** + * \brief serialize version for std::array + */ +template +inline void serialize(Stream& stream, const boost::array& t) +{ + BoostArraySerializer::write(stream, t); +} + +/** + * \brief deserialize version for std::array + */ +template +inline void deserialize(Stream& stream, boost::array& t) { + BoostArraySerializer::read(stream, t); +} + +/** + * \brief serializationLength version for std::array + */ +template +inline uint32_t serializationLength(const boost::array& t) +{ + return BoostArraySerializer::serializedLength(t); +} + +/** + * \brief UbloxStream base-class, provides common functionality for UbloxIStream and UbloxOStream + */ +struct UbloxStream +{ + /* + * \brief Returns a pointer to the current position of the stream + */ + inline uint8_t* getData() { + return data_; + } /** - * @brief Decode the message payload from the data buffer. - * @param data a pointer to the start of the message payload - * @param count the number of bytes in the message payload - * @param message the output message + * \brief Advances the stream, checking bounds, and returns a pointer to the position before it + * was advanced. + * \throws StreamOverrunException if len would take this stream past the end of its buffer */ - static void read(const uint8_t *data, uint32_t count, - T &message); + uint8_t* advance(uint32_t len) { + uint8_t* old_data = data_; + data_ += len; + if (data_ > end_) { + // Throwing directly here causes a significant speed hit due to the extra code generated + // for the throw statement + //throwStreamOverrun(); + } + return old_data; + } + /** - * @brief Get the length of the message payload in bytes. - * - * @details The payload does not include the header or checksum. - * @param message the message to get the length of - * @return the length of the message in bytes. + * \brief Returns the amount of space left in the stream + */ + inline uint32_t getLength() { return static_cast(end_ - data_); } + +protected: + UbloxStream(uint8_t* _data, uint32_t _count) + : data_(_data) + , end_(_data + _count) + {} + +private: + uint8_t* data_; + uint8_t* end_; +}; + +/** + * \brief Input stream + */ +struct UbloxIStream : public UbloxStream +{ + UbloxIStream(uint8_t* data, uint32_t count) + : UbloxStream(data, count) + {} + + /** + * \brief Deserialize an item from this input stream */ - static uint32_t serializedLength(const T &message); + template + void next(T& t) { + deserialize(*this, t); + } +}; + +/** + * \brief Output stream + */ +struct UbloxOStream : public UbloxStream +{ + UbloxOStream(uint8_t* data, uint32_t count) + : UbloxStream(data, count) + {} /** - * @brief Encode the message payload as a byte array. - * @param data a buffer to fill with the message payload bytes - * @param size the length of the buffer - * @param message the output message + * \brief Serialize an item to this output stream */ - static void write(uint8_t *data, uint32_t size, const T &message); + template + void next(const T& t) { + serialize(*this, t); + } }; /** @@ -302,7 +646,7 @@ class Reader { return false; } - Serializer::read(data_ + options_.header_length, length(), message); + UbloxSerializer::read(data_ + options_.header_length, length(), message); return true; } @@ -369,14 +713,14 @@ class Writer { uint8_t class_id = T::CLASS_ID, uint8_t message_id = T::MESSAGE_ID) { // Check for buffer overflow - uint32_t length = Serializer::serializedLength(message); + uint32_t length = UbloxSerializer::serializedLength(message); if (size_ < length + options_.wrapper_length()) { ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", class_id, message_id); return false; } // Encode the message and add it to the buffer - Serializer::write(data_ + options_.header_length, + UbloxSerializer::write(data_ + options_.header_length, size_ - options_.header_length, message); return write(0, length, class_id, message_id); } @@ -443,7 +787,7 @@ class Writer { // Use to declare u-blox messages and message serializers #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ - template class ublox::Serializer; \ + template class ublox::UbloxSerializer; \ template class ublox::Message; \ namespace package { namespace { \ static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ @@ -457,8 +801,4 @@ class Writer { static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ } } \ - -// use implementation of class Serializer in "serialization_ros.hpp" -#include "serialization_ros.hpp" - #endif // UBLOX_SERIALIZATION_SERIALIZATION_HPP diff --git a/ublox_serialization/include/ublox_serialization/serialization_ros.hpp b/ublox_serialization/include/ublox_serialization/serialization_ros.hpp deleted file mode 100644 index 0a7b2bc9..00000000 --- a/ublox_serialization/include/ublox_serialization/serialization_ros.hpp +++ /dev/null @@ -1,61 +0,0 @@ -//============================================================================== -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// 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 the Flight Systems and Automatic Control group, -// TU Darmstadt, 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 THE COPYRIGHT HOLDER 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 UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP -#define UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP - -#include - -#include - -#include "serialization.hpp" -#include "checksum.hpp" - -namespace ublox { - -template -void Serializer::read(const uint8_t *data, uint32_t count, - T &message) { - ros::serialization::IStream stream(const_cast(data), count); - ros::serialization::Serializer::read(stream, message); -} - -template -uint32_t Serializer::serializedLength(const T &message) { - return ros::serialization::Serializer::serializedLength(message); -} - -template -void Serializer::write(uint8_t *data, uint32_t size, const T &message) { - ros::serialization::OStream stream(data, size); - ros::serialization::Serializer::write(stream, message); -} - -} // namespace ublox - -#endif // UBLOX_SERIALIZATION_SERIALIZATION_ROS_HPP From 1cba3746f67277101f4f6916c983f61b9e78a008 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 19:46:34 +0000 Subject: [PATCH 028/152] Remove unused vector serialization. Signed-off-by: Chris Lalancette --- .../ublox_serialization/serialization.hpp | 77 ------------------- 1 file changed, 77 deletions(-) diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 98592aae..813eab7e 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -144,83 +144,6 @@ inline uint32_t serializationLength(const T& t) { return UbloxSerializer::serializedLength(t); } -/** - * \brief Vector serializer. Default implementation does nothing - */ -template -struct VectorUbloxSerializer -{}; - -/** - * \brief Vector serializer, specialized for fixed-size simple types - */ -template -struct VectorUbloxSerializer::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value>::type> -{ - typedef std::vector::other> VecType; - typedef typename VecType::iterator IteratorType; - typedef typename VecType::const_iterator ConstIteratorType; - - template - inline static void write(Stream& stream, const VecType& v) { - uint32_t len = static_cast(v.size()); - stream.next(len); - if (!v.empty()) { - const uint32_t data_len = len * static_cast(sizeof(T)); - memcpy(stream.advance(data_len), &v.front(), data_len); - } - } - - template - inline static void read(Stream& stream, VecType& v) { - uint32_t len; - stream.next(len); - v.resize(len); - - if (len > 0) { - const uint32_t data_len = static_cast(sizeof(T)) * len; - memcpy(&v.front(), stream.advance(data_len), data_len); - } - } - - inline static uint32_t serializedLength(const VecType& v) { - return 4 + v.size() * static_cast(sizeof(T)); - } -}; - -/** - * \brief serialize version for std::vector - */ -template -inline void serialize(Stream& stream, const std::vector& t) { - VectorUbloxSerializer::write(stream, t); -} - -/** - * \brief deserialize version for std::vector - */ -template -inline void deserialize(Stream& stream, std::vector& t) { - VectorUbloxSerializer::read(stream, t); -} - -/** - * \brief serializationLength version for std::vector - */ -template -inline uint32_t serializationLength(const std::vector& t) { - return VectorUbloxSerializer::serializedLength(t); -} - /** * \brief Array serializer, default implementation does nothing */ From 8f0023cbd2f71eddb810f53d96236218504f3044 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 20:15:01 +0000 Subject: [PATCH 029/152] Get rid of global 'debug' variable. It's now something we pass around. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 36 ++-- ublox_gps/include/ublox_gps/callback.hpp | 24 ++- ublox_gps/include/ublox_gps/gps.hpp | 6 +- ublox_gps/include/ublox_gps/node.hpp | 2 +- ublox_gps/src/gps.cpp | 18 +- ublox_gps/src/node.cpp | 186 ++++++++++--------- 6 files changed, 142 insertions(+), 130 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 5115d89e..e4933b7b 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -47,13 +47,11 @@ namespace ublox_gps { -int debug; //!< Used to determine which debug messages to display - /** * @brief Handles Asynchronous I/O reading and writing. */ template -class AsyncWorker : public Worker { +class AsyncWorker final : public Worker { public: /** * @brief Construct an Asynchronous I/O worker. @@ -61,38 +59,39 @@ class AsyncWorker : public Worker { * @param io_service the I/O service * @param buffer_size the size of the input and output buffers */ - AsyncWorker(std::shared_ptr stream, - std::shared_ptr io_service, - std::size_t buffer_size = 8192); - virtual ~AsyncWorker(); + explicit AsyncWorker(std::shared_ptr stream, + std::shared_ptr io_service, + std::size_t buffer_size = 8192, + int debug = 1); + ~AsyncWorker(); /** * @brief Set the callback function which handles input messages. * @param callback the read callback which handles received messages */ - void setCallback(const Callback& callback) { read_callback_ = callback; } + void setCallback(const Callback& callback) override { read_callback_ = callback; } /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Callback& callback) { write_callback_ = callback; } + void setRawDataCallback(const Callback& callback) override { write_callback_ = callback; } /** * @brief Send the data bytes via the I/O stream. * @param data the buffer of data bytes to send * @param size the size of the buffer */ - bool send(const unsigned char* data, const unsigned int size); + bool send(const unsigned char* data, const unsigned int size) override; /** * @brief Wait for incoming messages. * @param timeout the maximum time to wait */ - void wait(const std::chrono::milliseconds& timeout); + void wait(const std::chrono::milliseconds& timeout) override; - bool isOpen() const { return stream_->is_open(); } + bool isOpen() const override { return stream_->is_open(); } - protected: + private: /** * @brief Read the input stream. */ @@ -134,13 +133,16 @@ class AsyncWorker : public Worker { Callback write_callback_; //!< Callback function to handle raw data bool stopping_; //!< Whether or not the I/O service is closed + + bool debug_; //!< Used to determine which debug messages to display }; template AsyncWorker::AsyncWorker(std::shared_ptr stream, std::shared_ptr io_service, - std::size_t buffer_size) - : stopping_(false) { + std::size_t buffer_size, + int debug) + : stopping_(false), debug_(debug) { stream_ = stream; io_service_ = io_service; in_.resize(buffer_size); @@ -188,7 +190,7 @@ void AsyncWorker::doWrite() { // Write all the data in the out buffer asio::write(*stream_, asio::buffer(out_.data(), out_.size())); - if (debug >= 2) { + if (debug_ >= 2) { // Print the data that was sent std::ostringstream oss; for (std::vector::iterator it = out_.begin(); @@ -230,7 +232,7 @@ void AsyncWorker::readEnd(const asio::error_code& error, write_callback_(pRawDataStart, raw_data_stream_size); } - if (debug >= 4) { + if (debug_ >= 4) { std::ostringstream oss; for (std::vector::iterator it = in_.begin() + in_buffer_size_ - bytes_transfered; diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index c80aea13..20496979 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -70,7 +70,7 @@ class CallbackHandler { * @typedef T the message type */ template -class CallbackHandler_ : public CallbackHandler { +class CallbackHandler_ final : public CallbackHandler { public: typedef std::function Callback; //!< A callback function @@ -78,7 +78,7 @@ class CallbackHandler_ : public CallbackHandler { * @brief Initialize the Callback Handler with a callback function * @param func a callback function for the message, defaults to none */ - CallbackHandler_(const Callback& func = Callback()) : func_(func) {} + explicit CallbackHandler_(const Callback& func = Callback(), int debug = 1) : func_(func), debug_(debug) {} /** * @brief Get the last received message. @@ -89,11 +89,11 @@ class CallbackHandler_ : public CallbackHandler { * @brief Decode the U-Blox message & call the callback function if it exists. * @param reader a reader to decode the message buffer */ - void handle(ublox::Reader& reader) { + void handle(ublox::Reader& reader) override { std::lock_guard lock(mutex_); try { if (!reader.read(message_)) { - ROS_DEBUG_COND(debug >= 2, + ROS_DEBUG_COND(debug_ >= 2, "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), @@ -102,7 +102,7 @@ class CallbackHandler_ : public CallbackHandler { return; } } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug >= 2, + ROS_DEBUG_COND(debug_ >= 2, "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", static_cast(reader.classId()), static_cast(reader.messageId()), @@ -120,13 +120,16 @@ class CallbackHandler_ : public CallbackHandler { private: Callback func_; //!< the callback function to handle the message T message_; //!< The last received message + int debug_; }; /** * @brief Callback handlers for incoming u-blox messages. */ -class CallbackHandlers { +class CallbackHandlers final { public: + explicit CallbackHandlers(int debug) : debug_(debug) {} + /** * @brief Add a callback handler for the given message type. * @param callback the callback handler for the message @@ -137,7 +140,7 @@ class CallbackHandlers { std::lock_guard lock(callback_mutex_); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), - std::make_shared>(callback))); + std::make_shared>(callback, debug_))); } /** @@ -155,7 +158,7 @@ class CallbackHandlers { std::lock_guard lock(callback_mutex_); callbacks_.insert( std::make_pair(std::make_pair(T::CLASS_ID, message_id), - std::make_shared>(callback))); + std::make_shared>(callback, debug_))); } /** @@ -183,7 +186,7 @@ class CallbackHandlers { bool result = false; // Create a callback handler for this message callback_mutex_.lock(); - auto handler = std::make_shared>(); + auto handler = std::make_shared>(typename CallbackHandler_::Callback(), debug_); Callbacks::iterator callback = callbacks_.insert( (std::make_pair(std::make_pair(T::CLASS_ID, T::MESSAGE_ID), handler))); @@ -212,7 +215,7 @@ class CallbackHandlers { ublox::Reader reader(data, size); // Read all U-Blox messages in buffer while (reader.search() != reader.end() && reader.found()) { - if (debug >= 3) { + if (debug_ >= 3) { // Print the received bytes std::ostringstream oss; for (ublox::Reader::iterator it = reader.pos(); @@ -238,6 +241,7 @@ class CallbackHandlers { // Call back handlers for u-blox messages Callbacks callbacks_; std::mutex callback_mutex_; + int debug_; }; } // namespace ublox_gps diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 7e8b8f8e..b907354a 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -62,7 +62,7 @@ constexpr static unsigned int kBaudrates[] = { 4800, /** * @brief Handles communication with and configuration of the u-blox device */ -class Gps { +class Gps final { public: //! Sleep time [ms] after setting the baudrate int kSetBaudrateSleepMs = 500; @@ -71,7 +71,7 @@ class Gps { //! Size of write buffer for output messages constexpr static int kWriterSize = 2056; - Gps(); + explicit Gps(int debug); virtual ~Gps(); /** @@ -473,6 +473,8 @@ class Gps { //! Stores last received ACK accessed by multiple threads mutable std::atomic ack_; + int debug_; + //! Callback handlers for u-blox messages CallbackHandlers callbacks_; diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 3d24aae9..8bb2c469 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -92,7 +92,7 @@ std::shared_ptr updater; std::shared_ptr nh; //! Handles communication with the U-Blox Device -ublox_gps::Gps gps; +std::shared_ptr gps; //! Which GNSS are supported by the device std::set supported; //! Whether or not to publish the given ublox message diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index f9cb6c57..514fd3e1 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -46,7 +46,7 @@ const std::chrono::milliseconds Gps::default_timeout_ = std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps() : configured_(false), config_on_startup_flag_(true) { +Gps::Gps(int debug) : configured_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug) { subscribeAcks(); } @@ -82,7 +82,7 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - ROS_DEBUG_COND(debug >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", + ROS_DEBUG_COND(debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } @@ -105,7 +105,7 @@ void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { ack.msg_id = m.MESSAGE_ID; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - ROS_DEBUG_COND(ack.type == ACK && debug >= 2, + ROS_DEBUG_COND(ack.type == ACK && debug_ >= 2, "U-blox: received UPD SOS Backup ACK"); if (ack.type == NACK) { ROS_ERROR("U-blox: received UPD SOS Backup NACK"); @@ -139,7 +139,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service)); + setWorker(std::make_shared>(serial, io_service, 8192, debug_)); configured_ = false; @@ -190,7 +190,7 @@ void Gps::resetSerial(const std::string & port) { if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service)); + setWorker(std::make_shared>(serial, io_service, 8192, debug_)); configured_ = false; // Poll UART PRT Config @@ -244,7 +244,7 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { return; } setWorker(std::make_shared>(socket, - io_service)); + io_service, 8192, debug_)); } void Gps::close() { @@ -477,7 +477,7 @@ bool Gps::disableTmode3() { } bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - ROS_DEBUG_COND(debug >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, + ROS_DEBUG_COND(debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, message_id, rate); ublox_msgs::CfgMSG msg; msg.msg_class = class_id; @@ -556,7 +556,7 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug >= 2, "Waiting for ACK 0x%02x / 0x%02x", + ROS_DEBUG_COND(debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", class_id, msg_id); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::system_clock::time_point wait_until = now + timeout; @@ -591,7 +591,7 @@ bool Gps::setUTCtime() { } bool Gps::setTimtm2(uint8_t rate) { - ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate ); + ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate); ublox_msgs::CfgMSG msg; msg.msg_class = ublox_msgs::TimTM2::CLASS_ID; msg.msg_id = ublox_msgs::TimTM2::MESSAGE_ID; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 6af317ae..aac13d81 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -112,6 +112,9 @@ std::vector stringSplit(const std::string &str, // u-blox ROS Node // UbloxNode::UbloxNode() { + int debug; + nh->param("debug", debug, 1); + gps = std::make_shared(debug); nav_status_pub_ = nh->advertise("navstatus", kROSQueueSize); nav_posecef_pub_ = nh->advertise("navposecef", kROSQueueSize); nav_clock_pub_ = nh->advertise("navclock", kROSQueueSize); @@ -267,13 +270,13 @@ void UbloxNode::getRosParams() { void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); if (enabled["aid_alm"]) { - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); + gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } if (enabled["aid_eph"]) { - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); + gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } if (enabled["aid_hui"]) { - gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); + gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } payload[0]++; @@ -307,26 +310,26 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_status_pub_), kSubscribeRate); } nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_posecef_pub_), kSubscribeRate); } nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_clock_pub_), kSubscribeRate); } // INF messages nh->param("inf/debug", enabled["inf_debug"], false); if (enabled["inf_debug"]) { - gps.subscribeId( + gps->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); @@ -334,7 +337,7 @@ void UbloxNode::subscribe() { nh->param("inf/error", enabled["inf_error"], enabled["inf"]); if (enabled["inf_error"]) { - gps.subscribeId( + gps->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); @@ -342,7 +345,7 @@ void UbloxNode::subscribe() { nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); if (enabled["inf_notice"]) { - gps.subscribeId( + gps->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); @@ -350,7 +353,7 @@ void UbloxNode::subscribe() { nh->param("inf/test", enabled["inf_test"], enabled["inf"]); if (enabled["inf_test"]) { - gps.subscribeId( + gps->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); @@ -358,7 +361,7 @@ void UbloxNode::subscribe() { nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); if (enabled["inf_warning"]) { - gps.subscribeId( + gps->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); @@ -367,19 +370,19 @@ void UbloxNode::subscribe() { // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, aid_alm_pub_), kSubscribeRate); } nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, aid_eph_pub_), kSubscribeRate); } nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, aid_hui_pub_), kSubscribeRate); } @@ -406,7 +409,7 @@ void UbloxNode::initializeRosDiagnostics() { void UbloxNode::processMonVer() { ublox_msgs::MonVER monVer; - if (!gps.poll(monVer)) { + if (!gps->poll(monVer)) { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); } @@ -477,19 +480,19 @@ void UbloxNode::processMonVer() { bool UbloxNode::configureUblox() { try { - if (!gps.isInitialized()) + if (!gps->isInitialized()) throw std::runtime_error("Failed to initialize."); if (load_.load_mask != 0) { ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.load_mask); - if (!gps.configure(load_)) + if (!gps->configure(load_)) throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); if (load_.load_mask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); - gps.reset(wait); - if (!gps.isConfigured()) + gps->reset(wait); + if (!gps->isConfigured()) throw std::runtime_error(std::string("Failed to reset serial I/O") + "after loading I/O configurations from device memory."); } @@ -497,9 +500,9 @@ bool UbloxNode::configureUblox() { if (config_on_startup_flag_) { if (set_usb_) { - gps.configUsb(usb_tx_, usb_in_, usb_out_); + gps->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps.configRate(meas_rate, nav_rate)) { + if (!gps->configRate(meas_rate, nav_rate)) { std::stringstream ss; ss << "Failed to set measurement rate to " << meas_rate << "ms and navigation rate to " << nav_rate; @@ -507,29 +510,29 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (supportsGnss("SBAS")) { - if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { + if (!gps->configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if (!gps.setPpp(enable_ppp_)) { + if (!gps->setPpp(enable_ppp_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_ppp_) ? "enable" : "disable") + " PPP."); } - if (!gps.setDynamicModel(dmodel_)) { + if (!gps->setDynamicModel(dmodel_)) { throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); } - if (!gps.setFixMode(fmode_)) { + if (!gps->setFixMode(fmode_)) { throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); } - if (!gps.setDeadReckonLimit(dr_limit_)) { + if (!gps->setDeadReckonLimit(dr_limit_)) { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (set_dat_ && !gps.configure(cfg_dat_)) { + if (set_dat_ && !gps->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component @@ -542,7 +545,7 @@ bool UbloxNode::configureUblox() { if (save_.save_mask != 0) { ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); - if (!gps.configure(save_)) { + if (!gps->configure(save_)) { ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } } @@ -582,13 +585,13 @@ void UbloxNode::configureInf() { } ROS_DEBUG("Configuring INF messages"); - if (!gps.configure(msg)) { + if (!gps->configure(msg)) { ROS_WARN("Failed to configure INF messages"); } } void UbloxNode::initializeIo() { - gps.setConfigOnStartup(config_on_startup_flag_); + gps->setConfigOnStartup(config_on_startup_flag_); std::smatch match; if (std::regex_match(device_, match, @@ -599,17 +602,17 @@ void UbloxNode::initializeIo() { std::string port(match[3]); ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); - gps.initializeTcp(host, port); + gps->initializeTcp(host, port); } else { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } } else { - gps.initializeSerial(device_, baudrate_, uart_in_, uart_out_); + gps->initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging if (rawDataStreamPa_.isEnabled()) { - gps.setRawDataCallback( + gps->setRawDataCallback( std::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); rawDataStreamPa_.initialize(); } @@ -651,8 +654,8 @@ void UbloxNode::initialize() { } void UbloxNode::shutdown() { - if (gps.isInitialized()) { - gps.close(); + if (gps->isInitialized()) { + gps->close(); ROS_INFO("Closed connection to %s.", device_.c_str()); } } @@ -741,7 +744,7 @@ void UbloxFirmware6::getRosParams() { bool UbloxFirmware6::configureUblox() { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - if (set_nmea_ && !gps.configure(cfg_nmea_)) { + if (set_nmea_ && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -756,19 +759,19 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps.subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), kSubscribeRate); - gps.subscribe(std::bind( + gps->subscribe(std::bind( // Subscribe to Nav SOL &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav VELNED - gps.subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_svinfo_pub_), kNavSvInfoSubscribeRate); } @@ -776,7 +779,7 @@ void UbloxFirmware6::subscribe() { // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); } } @@ -1012,7 +1015,7 @@ void UbloxFirmware7::getRosParams() { bool UbloxFirmware7::configureUblox() { /** Configure the GNSS **/ ublox_msgs::CfgGNSS cfgGNSSRead; - if (gps.poll(cfgGNSSRead)) { + if (gps->poll(cfgGNSSRead)) { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); @@ -1034,7 +1037,7 @@ bool UbloxFirmware7::configureUblox() { block.max_trk_ch = block.MAX_TRK_CH_GLONASS; block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; cfgGNSSWrite.blocks.push_back(block); - if (!gps.configure(cfgGNSSWrite)) { + if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " GLONASS."); @@ -1049,7 +1052,7 @@ bool UbloxFirmware7::configureUblox() { block.max_trk_ch = block.MAX_TRK_CH_QZSS; block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { + if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + ((enable_glonass_) ? "enable" : "disable") + " QZSS."); @@ -1064,14 +1067,14 @@ bool UbloxFirmware7::configureUblox() { block.max_trk_ch = block.MAX_TRK_CH_SBAS; block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; - if (!gps.configure(cfgGNSSWrite)) { + if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if (set_nmea_ && !gps.configure(cfg_nmea_)) { + if (set_nmea_ && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1083,14 +1086,14 @@ void UbloxFirmware7::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps.subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_svinfo_pub_), kNavSvInfoSubscribeRate); } @@ -1098,7 +1101,7 @@ void UbloxFirmware7::subscribe() { // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); } } @@ -1115,7 +1118,7 @@ UbloxFirmware8::UbloxFirmware8() { void UbloxFirmware8::getRosParams() { // UPD SOS configuration nh->param("clear_bbr", clear_bbr_, false); - gps.setSaveOnShutdown(nh->param("save_on_shutdown", false)); + gps->setSaveOnShutdown(nh->param("save_on_shutdown", false)); // GNSS enable/disable nh->param("gnss/gps", enable_gps_, true); @@ -1239,7 +1242,7 @@ void UbloxFirmware8::getRosParams() { bool UbloxFirmware8::configureUblox() { if (clear_bbr_) { // clear flash memory - if (!gps.clearBbr()) { + if (!gps->clearBbr()) { ROS_ERROR("u-blox failed to clear flash memory"); } } @@ -1248,7 +1251,7 @@ bool UbloxFirmware8::configureUblox() { // // First, get the current GNSS configuration ublox_msgs::CfgGNSS cfg_gnss; - if (gps.poll(cfg_gnss)) { + if (gps->poll(cfg_gnss)) { ROS_DEBUG("Read GNSS config."); ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); @@ -1318,7 +1321,7 @@ bool UbloxFirmware8::configureUblox() { // since this requires a cold reset if (correct) { ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - } else if (!gps.configGnss(cfg_gnss, std::chrono::seconds(15))) { + } else if (!gps->configGnss(cfg_gnss, std::chrono::seconds(15))) { throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); } @@ -1326,7 +1329,7 @@ bool UbloxFirmware8::configureUblox() { // // NMEA config // - if (set_nmea_ && !gps.configure(cfg_nmea_)) { + if (set_nmea_ && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1337,27 +1340,27 @@ void UbloxFirmware8::subscribe() { // Whether to publish Nav PVT messages nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT - gps.subscribe( + gps->subscribe( std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), kSubscribeRate); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_sat_pub_), kNavSvInfoSubscribeRate); } // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); } // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_rtcm_pub_), kSubscribeRate); } } @@ -1379,28 +1382,28 @@ void RawDataProduct::subscribe() { // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); } // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); } // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_eph_pub_), kSubscribeRate); } // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_alm_pub_), kSubscribeRate); } } @@ -1451,7 +1454,7 @@ void AdrUdrProduct::getRosParams() { } bool AdrUdrProduct::configureUblox() { - if (!gps.setUseAdr(use_adr_)) { + if (!gps->setUseAdr(use_adr_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); } @@ -1464,45 +1467,45 @@ void AdrUdrProduct::subscribe() { // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, nav_att_pub_), kSubscribeRate); } // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, esf_ins_pub_), kSubscribeRate); } // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, esf_meas_pub_), kSubscribeRate); // also publish sensor_msgs::Imu - gps.subscribe(std::bind( + gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), kSubscribeRate); } // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, esf_raw_pub_), kSubscribeRate); } // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, esf_status_pub_), kSubscribeRate); } // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, hnr_pvt_pub_), kSubscribeRate); } } @@ -1658,23 +1661,23 @@ void HpgRefProduct::getRosParams() { bool HpgRefProduct::configureUblox() { // Configure TMODE3 if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if (!gps.disableTmode3()) { + if (!gps->disableTmode3()) { throw std::runtime_error("Failed to disable TMODE3."); } mode_ = DISABLED; } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if (!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, + if (!gps->configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); } - if (!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps->configRtcm(rtcm_ids, rtcm_rates)) { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { if (!svin_reset_) { ublox_msgs::NavSVIN nav_svin; - if (!gps.poll(nav_svin)) { + if (!gps->poll(nav_svin)) { throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); } @@ -1689,7 +1692,7 @@ bool HpgRefProduct::configureUblox() { return true; } ublox_msgs::NavPVT nav_pvt; - if (!gps.poll(nav_pvt)) { + if (!gps->poll(nav_pvt)) { throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); } @@ -1708,18 +1711,18 @@ bool HpgRefProduct::configureUblox() { meas_rate_temp = kDefaultMeasPeriod; } // Set nav rate to 1 Hz during survey in - if (!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { + if (!gps->configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); } // As recommended in the documentation, first disable, then set to survey in - if (!gps.disableTmode3()) { + if (!gps->disableTmode3()) { ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); } else { mode_ = DISABLED; } // Set to Survey in mode - if (!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { + if (!gps->configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { throw std::runtime_error("Failed to set TMODE3 to survey-in."); } mode_ = SURVEY_IN; @@ -1731,7 +1734,7 @@ void HpgRefProduct::subscribe() { // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In - gps.subscribe(std::bind( + gps->subscribe(std::bind( &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), kSubscribeRate); } @@ -1755,12 +1758,12 @@ bool HpgRefProduct::setTimeMode() { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if (!gps.configRate(meas_rate, nav_rate)) { + if (!gps->configRate(meas_rate, nav_rate)) { ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, "navigation rate to ", nav_rate); } // Enable the RTCM out messages - if (!gps.configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps->configRtcm(rtcm_ids, rtcm_rates)) { ROS_ERROR("Failed to configure RTCM IDs"); return false; } @@ -1831,7 +1834,7 @@ void HpgRovProduct::getRosParams() { bool HpgRovProduct::configureUblox() { // Configure the DGNSS - if (!gps.setDgnss(dgnss_mode_)) { + if (!gps->setDgnss(dgnss_mode_)) { throw std::runtime_error(std::string("Failed to Configure DGNSS")); } return true; @@ -1841,7 +1844,7 @@ void HpgRovProduct::subscribe() { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(std::bind( + gps->subscribe(std::bind( &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); } @@ -1913,7 +1916,7 @@ void HpPosRecProduct::subscribe() { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps.subscribe(std::bind( + gps->subscribe(std::bind( &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); // Whether to publish the Heading info from Nav Relative Position NED @@ -1970,11 +1973,11 @@ void TimProduct::getRosParams() { bool TimProduct::configureUblox() { uint8_t r = 1; // Configure the reciever - if (!gps.setUTCtime()) { + if (!gps->setUTCtime()) { throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); } - if (!gps.setTimtm2(r)) { + if (!gps->setTimtm2(r)) { throw std::runtime_error(std::string("Failed to Configure TIM Product")); } @@ -1987,7 +1990,7 @@ void TimProduct::subscribe() { // Subscribe to TIM-TM2 messages (Time mark messages) nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - gps.subscribe(std::bind( + gps->subscribe(std::bind( &TimProduct::callbackTimTM2, this, std::placeholders::_1), kSubscribeRate); ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); @@ -1995,14 +1998,14 @@ void TimProduct::subscribe() { // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); } // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { - gps.subscribe(std::bind( + gps->subscribe(std::bind( publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); } } @@ -2040,8 +2043,9 @@ void TimProduct::initializeRosDiagnostics() { int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); ublox_node::nh = std::make_shared("~"); - ublox_node::nh->param("debug", ublox_gps::debug, 1); - if (ublox_gps::debug) { + int debug; + ublox_node::nh->param("debug", debug, 1); + if (debug) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); From 0740b9e4249ffb3fd5624a47efc9890c9af6df48 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 20:26:59 +0000 Subject: [PATCH 030/152] Lots of small code updates throughout the GPS module. Just to improve code quality. No functional changes in here. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 1 + ublox_gps/include/ublox_gps/fts_product.hpp | 12 +- ublox_gps/include/ublox_gps/mkgmtime.h | 16 ++- ublox_gps/include/ublox_gps/node.hpp | 136 ++++++++++---------- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 9 +- ublox_gps/include/ublox_gps/utils.hpp | 10 +- 6 files changed, 94 insertions(+), 90 deletions(-) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 20496979..46c8e4ac 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp index 898474ab..6f378a50 100644 --- a/ublox_gps/include/ublox_gps/fts_product.hpp +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -43,7 +43,7 @@ class FtsProduct final : public virtual ComponentInterface { * @brief Get the FTS parameters. * @todo Currently unimplemented. */ - void getRosParams() { + void getRosParams() override { ROS_WARN("Functionality specific to u-blox FTS devices is %s", "unimplemented. See FtsProduct class in node.hpp & node.cpp."); } @@ -52,19 +52,19 @@ class FtsProduct final : public virtual ComponentInterface { * @brief Configure FTS settings. * @todo Currently unimplemented. */ - bool configureUblox() { return false; } + bool configureUblox() override { return false; } /** - * @brief Subscribe to FTS messages. + * @brief Adds diagnostic updaters for FTS status. * @todo Currently unimplemented. */ - void subscribe() {} + void initializeRosDiagnostics() override {} /** - * @brief Adds diagnostic updaters for FTS status. + * @brief Subscribe to FTS messages. * @todo Currently unimplemented. */ - void initializeRosDiagnostics() {} + void subscribe() override {} }; #endif diff --git a/ublox_gps/include/ublox_gps/mkgmtime.h b/ublox_gps/include/ublox_gps/mkgmtime.h index ba8142d8..900c1ca2 100644 --- a/ublox_gps/include/ublox_gps/mkgmtime.h +++ b/ublox_gps/include/ublox_gps/mkgmtime.h @@ -41,15 +41,23 @@ * */ -#ifndef INCLUDED_MKGMTIME_H -#define INCLUDED_MKGMTIME_H +#ifndef UBLOX_GPS_MKGMTIME_H +#define UBLOX_GPS_MKGMTIME_H #include +#ifdef __cplusplus +extern "C" { +#endif + /** * @brief Get the UTC time in seconds and nano-seconds from a time struct in * GM time. */ -extern time_t mkgmtime(struct tm * const tmp); +time_t mkgmtime(struct tm * const tmp); + +#ifdef __cplusplus +} +#endif -#endif /* INCLUDED_MKGMTIME_H */ \ No newline at end of file +#endif // UBLOX_GPS_MKGMTIME_H diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 8bb2c469..6546d954 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -633,7 +633,7 @@ class UbloxFirmware : public virtual ComponentInterface { /** * @brief Add the fix diagnostics to the updater. */ - void initializeRosDiagnostics(); + void initializeRosDiagnostics() override; protected: /** @@ -646,32 +646,32 @@ class UbloxFirmware : public virtual ComponentInterface { /** * @brief Implements functions for firmware version 6. */ -class UbloxFirmware6 : public UbloxFirmware { +class UbloxFirmware6 final : public UbloxFirmware { public: UbloxFirmware6(); /** * @brief Sets the fix status service type to GPS. */ - void getRosParams(); + void getRosParams() override; /** * @brief Prints a warning, GNSS configuration not available in this version. * @return true if configured correctly, false otherwise */ - bool configureUblox(); + bool configureUblox() override; /** * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. */ - void subscribe(); + void subscribe() override; protected: /** * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL * messages. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override; private: /** @@ -848,7 +848,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { /** * @brief Update the fix diagnostics from Nav PVT message. */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override { // check the last message, convert to diagnostic if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { @@ -914,7 +914,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { /** * @brief Implements functions for firmware version 7. */ -class UbloxFirmware7 : public UbloxFirmware7Plus { +class UbloxFirmware7 final : public UbloxFirmware7Plus { public: UbloxFirmware7(); @@ -923,19 +923,19 @@ class UbloxFirmware7 : public UbloxFirmware7Plus { * * @details Get the GNSS and NMEA settings. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure GNSS individually. Only configures GLONASS. */ - bool configureUblox(); + bool configureUblox() override; /** * @brief Subscribe to messages which are not generic to all firmware. * * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. */ - void subscribe(); + void subscribe() override; private: //! Used to configure NMEA (if set_nmea_) @@ -962,7 +962,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * * @details Get the GNSS, NMEA, and UPD settings. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure settings specific to firmware 8 based on ROS parameters. @@ -971,7 +971,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * Configure the NMEA if desired by the user. It also may clear the * flash memory based on the ROS parameters. */ - bool configureUblox(); + bool configureUblox() override; /** * @brief Subscribe to u-blox messages which are not generic to all firmware @@ -980,7 +980,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based * on user settings. */ - void subscribe(); + void subscribe() override; private: // Set from ROS parameters @@ -1007,13 +1007,13 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * For now it simply re-uses the firmware version 8 class * but allows for future expansion of functionality */ -class UbloxFirmware9 : public UbloxFirmware8 { +class UbloxFirmware9 final : public UbloxFirmware8 { }; /** * @brief Implements functions for Raw Data products. */ -class RawDataProduct: public virtual ComponentInterface { +class RawDataProduct final : public virtual ComponentInterface { public: double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; @@ -1023,25 +1023,25 @@ class RawDataProduct: public virtual ComponentInterface { /** * @brief Does nothing since there are no Raw Data product specific settings. */ - void getRosParams() {} + void getRosParams() override {} /** * @brief Does nothing since there are no Raw Data product specific settings. * @return always returns true */ - bool configureUblox() { return true; } + bool configureUblox() override { return true; } /** - * @brief Subscribe to Raw Data Product messages and set up ROS publishers. - * - * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. + * @brief Adds frequency diagnostics for RTCM topics. */ - void subscribe(); + void initializeRosDiagnostics() override; /** - * @brief Adds frequency diagnostics for RTCM topics. + * @brief Subscribe to Raw Data Product messages and set up ROS publishers. + * + * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. */ - void initializeRosDiagnostics(); + void subscribe() override; private: //! Topic diagnostic updaters @@ -1057,7 +1057,7 @@ class RawDataProduct: public virtual ComponentInterface { * @brief Implements functions for Automotive Dead Reckoning (ADR) and * Untethered Dead Reckoning (UDR) Devices. */ -class AdrUdrProduct: public virtual ComponentInterface { +class AdrUdrProduct final : public virtual ComponentInterface { public: AdrUdrProduct(); @@ -1066,33 +1066,33 @@ class AdrUdrProduct: public virtual ComponentInterface { * * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure ADR/UDR settings. * @details Configure the use_adr setting. * @return true if configured correctly, false otherwise */ - bool configureUblox(); - - /** - * @brief Subscribe to ADR/UDR messages. - * - * @details Subscribe to NavATT, ESF and HNR messages based on user - * parameters. - */ - void subscribe(); + bool configureUblox() override; /** * @brief Initialize the ROS diagnostics for the ADR/UDR device. * @todo unimplemented */ - void initializeRosDiagnostics() { + void initializeRosDiagnostics() override { ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); } - protected: + /** + * @brief Subscribe to ADR/UDR messages. + * + * @details Subscribe to NavATT, ESF and HNR messages based on user + * parameters. + */ + void subscribe() override; + + private: //! Whether or not to enable dead reckoning bool use_adr_; @@ -1128,7 +1128,7 @@ class HpgRefProduct: public virtual ComponentInterface { * tmode3 parameter is set to survey in or it will get the fixed parameters if * it is set to fixed. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure the u-blox Reference Station settings. @@ -1138,19 +1138,19 @@ class HpgRefProduct: public virtual ComponentInterface { * the RTCM messages. * @return true if configured correctly, false otherwise */ - bool configureUblox(); + bool configureUblox() override; /** - * @brief Subscribe to u-blox Reference Station messages. - * - * @details Subscribe to NavSVIN messages based on user parameters. + * @brief Add diagnostic updaters for the TMODE3 status. */ - void subscribe(); + void initializeRosDiagnostics() override; /** - * @brief Add diagnostic updaters for the TMODE3 status. + * @brief Subscribe to u-blox Reference Station messages. + * + * @details Subscribe to NavSVIN messages based on user parameters. */ - void initializeRosDiagnostics(); + void subscribe() override; /** * @brief Update the last received NavSVIN message and call diagnostic updater @@ -1228,7 +1228,7 @@ class HpgRefProduct: public virtual ComponentInterface { /** * @brief Implements functions for High Precision GNSS Rover devices. */ -class HpgRovProduct: public virtual ComponentInterface { +class HpgRovProduct final : public virtual ComponentInterface { public: // Constants for diagnostic updater //! Diagnostic updater: RTCM topic frequency min [Hz] @@ -1247,7 +1247,7 @@ class HpgRovProduct: public virtual ComponentInterface { * * @details Get the DGNSS mode. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure rover settings. @@ -1255,20 +1255,20 @@ class HpgRovProduct: public virtual ComponentInterface { * @details Configure the DGNSS mode. * @return true if configured correctly, false otherwise */ - bool configureUblox(); + bool configureUblox() override; /** - * @brief Subscribe to Rover messages, such as NavRELPOSNED. + * @brief Add diagnostic updaters for rover GNSS status, including + * status of RTCM messages. */ - void subscribe(); + void initializeRosDiagnostics() override; /** - * @brief Add diagnostic updaters for rover GNSS status, including - * status of RTCM messages. + * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ - void initializeRosDiagnostics(); + void subscribe() override; - protected: + private: /** * @brief Update the rover diagnostics, including the carrier phase solution * status (float or fixed). @@ -1297,16 +1297,16 @@ class HpgRovProduct: public virtual ComponentInterface { ros::Publisher nav_rel_pos_ned_pub_; }; -class HpPosRecProduct: public virtual HpgRefProduct { +class HpPosRecProduct final : public virtual HpgRefProduct { public: HpPosRecProduct(); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ - void subscribe(); + void subscribe() override; - protected: + private: /** * @brief Set the last received message and call rover diagnostic updater @@ -1328,7 +1328,7 @@ class HpPosRecProduct: public virtual HpgRefProduct { * @brief Implements functions for Time Sync products. * @todo partially implemented */ -class TimProduct: public virtual ComponentInterface { +class TimProduct final : public virtual ComponentInterface { public: TimProduct(); @@ -1336,28 +1336,28 @@ class TimProduct: public virtual ComponentInterface { * @brief Get the Time Sync parameters. * @todo Currently unimplemented. */ - void getRosParams(); + void getRosParams() override; /** * @brief Configure Time Sync settings. * @todo Currently unimplemented. */ - bool configureUblox(); + bool configureUblox() override; /** - * @brief Subscribe to Time Sync messages. - * - * @details Subscribes to RxmRAWX & RxmSFRBX messages. + * @brief Adds diagnostic updaters for Time Sync status. + * @todo Currently unimplemented. */ - void subscribe(); + void initializeRosDiagnostics() override; /** - * @brief Adds diagnostic updaters for Time Sync status. - * @todo Currently unimplemented. + * @brief Subscribe to Time Sync messages. + * + * @details Subscribes to RxmRAWX & RxmSFRBX messages. */ - void initializeRosDiagnostics(); + void subscribe() override; - protected: + private: /** * @brief * @details Publish recieved TimTM2 messages if enabled diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index f4773d21..3ae74076 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -33,13 +33,12 @@ // measured data with the rtklib. -#ifndef UBLOX_RAW_DATA_PA_H -#define UBLOX_RAW_DATA_PA_H +#ifndef UBLOX_GPS_RAW_DATA_PA_HPP +#define UBLOX_GPS_RAW_DATA_PA_HPP // STL #include -#include -#include +#include // ROS includes #include @@ -57,7 +56,7 @@ namespace ublox_node { /** * @brief Implements functions for raw data stream. */ -class RawDataStreamPa { +class RawDataStreamPa final { public: diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 1868031a..bfec002c 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -1,13 +1,9 @@ -#ifndef UBLOX_GPS_UTILS_H -#define UBLOX_GPS_UTILS_H +#ifndef UBLOX_GPS_UTILS_HPP +#define UBLOX_GPS_UTILS_HPP -#include #include -#include "ublox_msgs/NavPVT.h" -extern "C" { - #include "ublox_gps/mkgmtime.h" -} +#include "ublox_gps/mkgmtime.h" /** * @brief Convert date/time to UTC time in seconds. From 1e22553ba64cbd1d60d5e51d936f54f232b42044 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 21:30:16 +0000 Subject: [PATCH 031/152] Make nav_rate a class variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 34 +++++++++++++------- ublox_gps/src/node.cpp | 48 ++++++++++++++-------------- 2 files changed, 46 insertions(+), 36 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 6546d954..fd82c63c 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -108,8 +108,6 @@ std::string frame_id; int fix_status_service; //! The measurement [ms], see CfgRate.msg uint16_t meas_rate; -//! Navigation rate in measurement cycles, see CfgRate.msg -uint16_t nav_rate; //! IDs of RTCM out messages to configure. std::vector rtcm_ids; //! Rates of RTCM out messages. Size must be the same as rtcm_ids @@ -133,7 +131,7 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window) { + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, uint16_t nav_rate) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -154,8 +152,8 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, + double freq_tol, int freq_window) { min_freq = freq_min; max_freq = freq_max; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, @@ -189,8 +187,8 @@ struct FixDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics * @param stamp_min the minimum allowed time delay */ - FixDiagnostic(const std::string & name, double freq_tol, int freq_window, - double stamp_min) { + explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, + double stamp_min, uint16_t nav_rate) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -621,6 +619,9 @@ class UbloxNode final { ros::Publisher aid_alm_pub_; ros::Publisher aid_eph_pub_; ros::Publisher aid_hui_pub_; + + //! Navigation rate in measurement cycles, see CfgRate.msg + uint16_t nav_rate_; }; /** @@ -1018,7 +1019,7 @@ class RawDataProduct final : public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; - RawDataProduct(); + explicit RawDataProduct(uint16_t nav_rate); /** * @brief Does nothing since there are no Raw Data product specific settings. @@ -1051,6 +1052,8 @@ class RawDataProduct final : public virtual ComponentInterface { ros::Publisher rxm_sfrb_pub_; ros::Publisher rxm_eph_pub_; ros::Publisher rxm_alm_pub_; + + uint16_t nav_rate_; }; /** @@ -1059,7 +1062,7 @@ class RawDataProduct final : public virtual ComponentInterface { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - AdrUdrProduct(); + explicit AdrUdrProduct(uint16_t nav_rate); /** * @brief Get the ADR/UDR parameters. @@ -1110,6 +1113,8 @@ class AdrUdrProduct final : public virtual ComponentInterface { ros::Publisher esf_raw_pub_; ros::Publisher esf_status_pub_; ros::Publisher hnr_pvt_pub_; + + uint16_t nav_rate_; }; /** @@ -1118,7 +1123,8 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: - HpgRefProduct(); + explicit HpgRefProduct(uint16_t nav_rate); + /** * @brief Get the ROS parameters specific to the Reference Station * configuration. @@ -1223,6 +1229,8 @@ class HpgRefProduct: public virtual ComponentInterface { } mode_; ros::Publisher navsvin_pub_; + + uint16_t nav_rate_; }; /** @@ -1240,7 +1248,7 @@ class HpgRovProduct final : public virtual ComponentInterface { //! Diagnostic updater: RTCM topic frequency window [num messages] constexpr static int kRtcmFreqWindow = 25; - HpgRovProduct(); + explicit HpgRovProduct(uint16_t nav_rate); /** * @brief Get the ROS parameters specific to the Rover configuration. @@ -1295,11 +1303,13 @@ class HpgRovProduct final : public virtual ComponentInterface { UbloxTopicDiagnostic freq_rtcm_; ros::Publisher nav_rel_pos_ned_pub_; + + uint16_t nav_rate_; }; class HpPosRecProduct final : public virtual HpgRefProduct { public: - HpPosRecProduct(); + explicit HpPosRecProduct(uint16_t nav_rate); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index aac13d81..905b0e45 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -147,16 +147,16 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -198,7 +198,7 @@ void UbloxNode::getRosParams() { } // Measurement rate params nh->param("rate", rate_, 4.0); // in Hz - getRosUint("nav_rate", nav_rate, 1); // # of measurement rate cycles + getRosUint("nav_rate", nav_rate_, 1); // # of measurement rate cycles // RTCM params getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates @@ -401,7 +401,7 @@ void UbloxNode::initializeRosDiagnostics() { // configure diagnostic updater for frequency freq_diag = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin); + kFixFreqWindow, kTimeStampStatusMin, nav_rate_); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } @@ -502,10 +502,10 @@ bool UbloxNode::configureUblox() { if (set_usb_) { gps->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps->configRate(meas_rate, nav_rate)) { + if (!gps->configRate(meas_rate, nav_rate_)) { std::stringstream ss; ss << "Failed to set measurement rate to " << meas_rate - << "ms and navigation rate to " << nav_rate; + << "ms and navigation rate to " << nav_rate_; throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) @@ -626,7 +626,7 @@ void UbloxNode::initialize() { processMonVer(); if (protocol_version_ <= 14) { if (nh->param("raw_data", false)) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(nav_rate_)); } } // Must set firmware & hardware params before initializing diagnostics @@ -1368,7 +1368,7 @@ void UbloxFirmware8::subscribe() { // // Raw Data Products // -RawDataProduct::RawDataProduct() { +RawDataProduct::RawDataProduct(uint16_t nav_rate) : nav_rate_(nav_rate) { rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); rxm_eph_pub_ = nh->advertise("rxmeph", kROSQueueSize); @@ -1411,26 +1411,26 @@ void RawDataProduct::subscribe() { void RawDataProduct::initializeRosDiagnostics() { if (enabled["rxm_raw"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmraw", kRtcmFreqTol, kRtcmFreqWindow)); + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); } if (enabled["rxm_sfrb"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow)); + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); } if (enabled["rxm_eph"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmeph", kRtcmFreqTol, kRtcmFreqWindow)); + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); } if (enabled["rxm_alm"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmalm", kRtcmFreqTol, kRtcmFreqWindow)); + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); } } // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct() +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate) : nav_rate_(nav_rate) { imu_pub_ = nh->advertise("imu_meas", kROSQueueSize); @@ -1447,7 +1447,7 @@ AdrUdrProduct::AdrUdrProduct() void AdrUdrProduct::getRosParams() { nh->param("use_adr", use_adr_, true); // Check the nav rate - float nav_rate_hz = 1000 / (meas_rate * nav_rate); + float nav_rate_hz = 1000 / (meas_rate * nav_rate_); if (nav_rate_hz != 1) { ROS_WARN("Nav Rate recommended to be 1 Hz"); } @@ -1607,7 +1607,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct() +HpgRefProduct::HpgRefProduct(uint16_t nav_rate) : nav_rate_(nav_rate) { navsvin_pub_ = nh->advertise("navsvin", kROSQueueSize); @@ -1615,7 +1615,7 @@ HpgRefProduct::HpgRefProduct() void HpgRefProduct::getRosParams() { if (config_on_startup_flag_) { - if (nav_rate * meas_rate != 1000) { + if (nav_rate_ * meas_rate != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); } @@ -1758,9 +1758,9 @@ bool HpgRefProduct::setTimeMode() { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if (!gps->configRate(meas_rate, nav_rate)) { + if (!gps->configRate(meas_rate, nav_rate_)) { ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, - "navigation rate to ", nav_rate); + "navigation rate to ", nav_rate_); } // Enable the RTCM out messages if (!gps->configRtcm(rtcm_ids, rtcm_rates)) { @@ -1820,7 +1820,7 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // -HpgRovProduct::HpgRovProduct() +HpgRovProduct::HpgRovProduct(uint16_t nav_rate) : nav_rate_(nav_rate) { nav_rel_pos_ned_pub_ = nh->advertise("navrelposned", kROSQueueSize); @@ -1850,8 +1850,8 @@ void HpgRovProduct::subscribe() { void HpgRovProduct::initializeRosDiagnostics() { freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); + kRtcmFreqMin, kRtcmFreqMax, + kRtcmFreqTol, kRtcmFreqWindow); updater->add("Carrier Phase Solution", this, &HpgRovProduct::carrierPhaseDiagnostics); updater->force_update(); @@ -1903,7 +1903,7 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct() +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate) : HpgRefProduct(nav_rate) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); From 4ce3160ca2ed6d114ee7c30fd77fc96342db06ca Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 21:38:04 +0000 Subject: [PATCH 032/152] Make meas_rate a member variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 21 ++++++++------ ublox_gps/src/node.cpp | 42 ++++++++++++++-------------- 2 files changed, 34 insertions(+), 29 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index fd82c63c..fe74c788 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -106,8 +106,6 @@ std::string frame_id; //! The fix status service type, set in the Firmware Component //! based on the enabled GNSS int fix_status_service; -//! The measurement [ms], see CfgRate.msg -uint16_t meas_rate; //! IDs of RTCM out messages to configure. std::vector rtcm_ids; //! Rates of RTCM out messages. Size must be the same as rtcm_ids @@ -131,7 +129,8 @@ struct UbloxTopicDiagnostic { * @param freq_tol the tolerance [%] for the topic frequency * @param freq_window the number of messages to use for diagnostic statistics */ - explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, uint16_t nav_rate) { + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, + uint16_t nav_rate, uint16_t meas_rate) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -188,7 +187,7 @@ struct FixDiagnostic { * @param stamp_min the minimum allowed time delay */ explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, - double stamp_min, uint16_t nav_rate) { + double stamp_min, uint16_t nav_rate, uint16_t meas_rate) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -622,6 +621,9 @@ class UbloxNode final { //! Navigation rate in measurement cycles, see CfgRate.msg uint16_t nav_rate_; + + //! The measurement [ms], see CfgRate.msg + uint16_t meas_rate_; }; /** @@ -1019,7 +1021,7 @@ class RawDataProduct final : public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; - explicit RawDataProduct(uint16_t nav_rate); + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate); /** * @brief Does nothing since there are no Raw Data product specific settings. @@ -1054,6 +1056,7 @@ class RawDataProduct final : public virtual ComponentInterface { ros::Publisher rxm_alm_pub_; uint16_t nav_rate_; + uint16_t meas_rate_; }; /** @@ -1062,7 +1065,7 @@ class RawDataProduct final : public virtual ComponentInterface { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate); /** * @brief Get the ADR/UDR parameters. @@ -1115,6 +1118,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { ros::Publisher hnr_pvt_pub_; uint16_t nav_rate_; + uint16_t meas_rate_; }; /** @@ -1123,7 +1127,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: - explicit HpgRefProduct(uint16_t nav_rate); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1231,6 +1235,7 @@ class HpgRefProduct: public virtual ComponentInterface { ros::Publisher navsvin_pub_; uint16_t nav_rate_; + uint16_t meas_rate_; }; /** @@ -1309,7 +1314,7 @@ class HpgRovProduct final : public virtual ComponentInterface { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 905b0e45..f1ba1470 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -147,16 +147,16 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_)); } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_)); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -258,7 +258,7 @@ void UbloxNode::getRosParams() { } // measurement period [ms] - meas_rate = 1000 / rate_; + meas_rate_ = 1000 / rate_; // activate/deactivate any config nh->param("config_on_startup", config_on_startup_flag_, true); @@ -401,7 +401,7 @@ void UbloxNode::initializeRosDiagnostics() { // configure diagnostic updater for frequency freq_diag = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin, nav_rate_); + kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } @@ -502,9 +502,9 @@ bool UbloxNode::configureUblox() { if (set_usb_) { gps->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps->configRate(meas_rate, nav_rate_)) { + if (!gps->configRate(meas_rate_, nav_rate_)) { std::stringstream ss; - ss << "Failed to set measurement rate to " << meas_rate + ss << "Failed to set measurement rate to " << meas_rate_ << "ms and navigation rate to " << nav_rate_; throw std::runtime_error(ss.str()); } @@ -626,7 +626,7 @@ void UbloxNode::initialize() { processMonVer(); if (protocol_version_ <= 14) { if (nh->param("raw_data", false)) { - components_.push_back(std::make_shared(nav_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_)); } } // Must set firmware & hardware params before initializing diagnostics @@ -1368,7 +1368,7 @@ void UbloxFirmware8::subscribe() { // // Raw Data Products // -RawDataProduct::RawDataProduct(uint16_t nav_rate) : nav_rate_(nav_rate) { +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) { rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); rxm_eph_pub_ = nh->advertise("rxmeph", kROSQueueSize); @@ -1411,26 +1411,26 @@ void RawDataProduct::subscribe() { void RawDataProduct::initializeRosDiagnostics() { if (enabled["rxm_raw"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); } if (enabled["rxm_sfrb"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); } if (enabled["rxm_eph"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); } if (enabled["rxm_alm"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_)); + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); } } // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate) : nav_rate_(nav_rate) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) { imu_pub_ = nh->advertise("imu_meas", kROSQueueSize); @@ -1447,7 +1447,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate) : nav_rate_(nav_rate) void AdrUdrProduct::getRosParams() { nh->param("use_adr", use_adr_, true); // Check the nav rate - float nav_rate_hz = 1000 / (meas_rate * nav_rate_); + float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { ROS_WARN("Nav Rate recommended to be 1 Hz"); } @@ -1607,7 +1607,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate) : nav_rate_(nav_rate) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) { navsvin_pub_ = nh->advertise("navsvin", kROSQueueSize); @@ -1615,7 +1615,7 @@ HpgRefProduct::HpgRefProduct(uint16_t nav_rate) : nav_rate_(nav_rate) void HpgRefProduct::getRosParams() { if (config_on_startup_flag_) { - if (nav_rate_ * meas_rate != 1000) { + if (nav_rate_ * meas_rate_ != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); } @@ -1705,7 +1705,7 @@ bool HpgRefProduct::configureUblox() { } // Reset the Survey In // For Survey in, meas rate must be at least 1 Hz - uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000; // [ms] + uint16_t meas_rate_temp = meas_rate_ < 1000 ? meas_rate_ : 1000; // [ms] // If measurement period isn't a factor of 1000, set to default if (1000 % meas_rate_temp != 0) { meas_rate_temp = kDefaultMeasPeriod; @@ -1758,8 +1758,8 @@ bool HpgRefProduct::setTimeMode() { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if (!gps->configRate(meas_rate, nav_rate_)) { - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate, + if (!gps->configRate(meas_rate_, nav_rate_)) { + ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate_, "navigation rate to ", nav_rate_); } // Enable the RTCM out messages @@ -1903,7 +1903,7 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate) : HpgRefProduct(nav_rate) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate) : HpgRefProduct(nav_rate, meas_rate) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); From ddf631efd7369f98fc9e416233b9c09dfe79d079 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 21:42:12 +0000 Subject: [PATCH 033/152] Make config_on_startup_flag a member variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 10 ++++++---- ublox_gps/src/node.cpp | 10 ++++++---- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index fe74c788..04a02caa 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -110,8 +110,6 @@ int fix_status_service; std::vector rtcm_ids; //! Rates of RTCM out messages. Size must be the same as rtcm_ids std::vector rtcm_rates; -//! Flag for enabling configuration on startup -bool config_on_startup_flag_; //! Topic diagnostics for u-blox messages @@ -624,6 +622,9 @@ class UbloxNode final { //! The measurement [ms], see CfgRate.msg uint16_t meas_rate_; + + //! Flag for enabling configuration on startup + bool config_on_startup_flag_; }; /** @@ -1127,7 +1128,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1236,6 +1237,7 @@ class HpgRefProduct: public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; + bool config_on_startup_flag_; }; /** @@ -1314,7 +1316,7 @@ class HpgRovProduct final : public virtual ComponentInterface { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f1ba1470..e1824f15 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -147,11 +147,11 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_)); } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("ADR") == 0 || @@ -1607,7 +1607,8 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag) + : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag) { navsvin_pub_ = nh->advertise("navsvin", kROSQueueSize); @@ -1903,7 +1904,8 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate) : HpgRefProduct(nav_rate, meas_rate) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag) + : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); From c7cab7771fbb66b4996f65f26b9f75493bff1e5f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 21:53:21 +0000 Subject: [PATCH 034/152] Make frame_id a member variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 42 ++++++++++++++------ ublox_gps/src/node.cpp | 58 +++++++++++++--------------- 2 files changed, 57 insertions(+), 43 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 04a02caa..42d73a85 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -101,8 +101,6 @@ std::set supported; * (e.g. NavPVT instead of NavPVT7). Value indicates whether or not to enable * the message. */ std::map enabled; -//! The ROS frame ID of this device -std::string frame_id; //! The fix status service type, set in the Firmware Component //! based on the enabled GNSS int fix_status_service; @@ -625,6 +623,9 @@ class UbloxNode final { //! Flag for enabling configuration on startup bool config_on_startup_flag_; + + //! The ROS frame ID of this device + std::string frame_id_; }; /** @@ -652,7 +653,7 @@ class UbloxFirmware : public virtual ComponentInterface { */ class UbloxFirmware6 final : public UbloxFirmware { public: - UbloxFirmware6(); + explicit UbloxFirmware6(const std::string & frame_id); /** * @brief Sets the fix status service type to GPS. @@ -726,6 +727,8 @@ class UbloxFirmware6 final : public UbloxFirmware { ros::Publisher nav_sol_pub_; ros::Publisher nav_svinfo_pub_; ros::Publisher mon_hw_pub_; + + std::string frame_id_; }; /** @@ -740,7 +743,7 @@ class UbloxFirmware6 final : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: - UbloxFirmware7Plus() { + explicit UbloxFirmware7Plus(const std::string & frame_id) : frame_id_(frame_id) { // NavPVT publisher nav_pvt_pub_ = nh->advertise("navpvt", kROSQueueSize); @@ -769,7 +772,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { // NavSatFix message // sensor_msgs::NavSatFix fix; - fix.header.frame_id = frame_id; + fix.header.frame_id = frame_id_; // set the timestamp uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; if (((m.valid & valid_time) == valid_time) && @@ -823,7 +826,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { // geometry_msgs::TwistWithCovarianceStamped velocity; velocity.header.stamp = fix.header.stamp; - velocity.header.frame_id = frame_id; + velocity.header.frame_id = frame_id_; // convert to XYZ linear velocity [m/s] in ENU velocity.twist.twist.linear.x = m.vel_e * 1e-3; @@ -913,6 +916,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { ros::Publisher nav_pvt_pub_; ros::Publisher fix_pub_; ros::Publisher vel_pub_; + + std::string frame_id_; }; /** @@ -920,7 +925,10 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - UbloxFirmware7(); + explicit UbloxFirmware7(const std::string & frame_id) : UbloxFirmware7Plus(frame_id) { + nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); + mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); + } /** * @brief Get the parameters specific to firmware version 7. @@ -959,7 +967,11 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { */ class UbloxFirmware8 : public UbloxFirmware7Plus { public: - UbloxFirmware8(); + explicit UbloxFirmware8(const std::string & frame_id) : UbloxFirmware7Plus(frame_id) { + nav_sat_pub_ = nh->advertise("navsate", kROSQueueSize); + mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); + rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); + } /** * @brief Get the ROS parameters specific to firmware version 8. @@ -1012,6 +1024,8 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * but allows for future expansion of functionality */ class UbloxFirmware9 final : public UbloxFirmware8 { +public: + explicit UbloxFirmware9(const std::string & frame_id); }; /** @@ -1066,7 +1080,7 @@ class RawDataProduct final : public virtual ComponentInterface { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id); /** * @brief Get the ADR/UDR parameters. @@ -1120,6 +1134,8 @@ class AdrUdrProduct final : public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; + + std::string frame_id_; }; /** @@ -1316,7 +1332,7 @@ class HpgRovProduct final : public virtual ComponentInterface { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. @@ -1339,6 +1355,8 @@ class HpPosRecProduct final : public virtual HpgRefProduct { ros::Publisher nav_relposned_pub_; ros::Publisher imu_pub_; + + std::string frame_id_; }; /** @@ -1347,7 +1365,7 @@ class HpPosRecProduct final : public virtual HpgRefProduct { */ class TimProduct final : public virtual ComponentInterface { public: - TimProduct(); + explicit TimProduct(const std::string & frame_id); /** * @brief Get the Time Sync parameters. @@ -1387,6 +1405,8 @@ class TimProduct final : public virtual ComponentInterface { ros::Publisher interrupt_time_pub_; ros::Publisher rxm_sfrb_pub_; ros::Publisher rxm_raw_pub_; + + std::string frame_id_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index e1824f15..f7661477 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -127,16 +127,16 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(frame_id_)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(frame_id_)); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(frame_id_)); ublox_version = 8; } else { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(frame_id_)); ublox_version = 9; } @@ -151,12 +151,12 @@ void UbloxNode::addProductInterface(const std::string & product_category, } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared(nav_rate_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_)); } else if (product_category.compare("TIM") == 0) { - components_.push_back(std::make_shared()); + components_.push_back(std::make_shared(frame_id_)); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_)); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -168,7 +168,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, void UbloxNode::getRosParams() { nh->param("device", device_, std::string("/dev/ttyACM0")); - nh->param("frame_id", frame_id, std::string("gps")); + nh->param("frame_id", frame_id_, std::string("gps")); // Save configuration parameters getRosUint("load/mask", load_.load_mask, 0); @@ -671,7 +671,7 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6() +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id) : frame_id_(frame_id) { nav_pos_llh_pub_ = nh->advertise("navposllh", kROSQueueSize); @@ -838,7 +838,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_.header.stamp = ros::Time::now(); // new timestamp } - fix_.header.frame_id = frame_id; + fix_.header.frame_id = frame_id_; fix_.latitude = m.lat * 1e-7; fix_.longitude = m.lon * 1e-7; fix_.altitude = m.height * 1e-3; @@ -878,7 +878,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { } else { velocity_.header.stamp = ros::Time::now(); // create a new timestamp } - velocity_.header.frame_id = frame_id; + velocity_.header.frame_id = frame_id_; // convert to XYZ linear velocity velocity_.twist.twist.linear.x = m.vel_e / 100.0; @@ -907,11 +907,6 @@ void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { // // Ublox Firmware Version 7 // -UbloxFirmware7::UbloxFirmware7() { - nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); - mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); -} - void UbloxFirmware7::getRosParams() { // // GNSS configuration @@ -1109,12 +1104,6 @@ void UbloxFirmware7::subscribe() { // // Ublox Version 8 // -UbloxFirmware8::UbloxFirmware8() { - nav_sat_pub_ = nh->advertise("navsate", kROSQueueSize); - mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); - rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); -} - void UbloxFirmware8::getRosParams() { // UPD SOS configuration nh->param("clear_bbr", clear_bbr_, false); @@ -1365,6 +1354,10 @@ void UbloxFirmware8::subscribe() { } } +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id) : UbloxFirmware8(frame_id) +{ +} + // // Raw Data Products // @@ -1430,7 +1423,8 @@ void RawDataProduct::initializeRosDiagnostics() { // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id) + : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id) { imu_pub_ = nh->advertise("imu_meas", kROSQueueSize); @@ -1513,7 +1507,7 @@ void AdrUdrProduct::subscribe() { void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { if (enabled["esf_meas"]) { imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id; + imu_.header.frame_id = frame_id_; float deg_per_sec = pow(2, -12); float m_per_sec_sq = pow(2, -10); @@ -1585,7 +1579,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // create time ref message and put in the data //t_ref_.header.seq = m.risingEdgeCount; //t_ref_.header.stamp = ros::Time::now(); - //t_ref_.header.frame_id = frame_id; + //t_ref_.header.frame_id = frame_id_; //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); @@ -1594,7 +1588,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { //t_ref_.source = src.str(); t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id; + t_ref_.header.frame_id = frame_id_; time_ref_pub_.publish(t_ref_); imu_pub_.publish(imu_); @@ -1904,8 +1898,8 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag) - : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id) + : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag), frame_id_(frame_id) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); @@ -1932,7 +1926,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { if (enabled["nav_heading"]) { imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id; + imu_.header.frame_id = frame_id_; imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; @@ -1959,7 +1953,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // // U-Blox Time Sync Products, partially implemented. // -TimProduct::TimProduct() +TimProduct::TimProduct(const std::string & frame_id) : frame_id_(frame_id) { timtm2_pub_ = nh->advertise("timtm2", kROSQueueSize); @@ -2018,7 +2012,7 @@ void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { // create time ref message and put in the data t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); - t_ref_.header.frame_id = frame_id; + t_ref_.header.frame_id = frame_id_; t_ref_.time_ref = ros::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); @@ -2027,7 +2021,7 @@ void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { t_ref_.source = src.str(); t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id; + t_ref_.header.frame_id = frame_id_; timtm2_pub_.publish(m); interrupt_time_pub_.publish(t_ref_); From 86f844ba011851e85571e095bfada22384bf398e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:24:07 +0000 Subject: [PATCH 035/152] Make updater a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 50 +++++++++------ ublox_gps/src/node.cpp | 96 +++++++++++++++------------- 2 files changed, 85 insertions(+), 61 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 42d73a85..2de0e586 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -86,8 +86,6 @@ constexpr static uint32_t kSubscribeRate = 1; constexpr static uint32_t kNavSvInfoSubscribeRate = 20; // ROS objects -//! ROS diagnostic updater -std::shared_ptr updater; //! Node Handle for GPS node std::shared_ptr nh; @@ -126,7 +124,7 @@ struct UbloxTopicDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics */ explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, - uint16_t nav_rate, uint16_t meas_rate) { + uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -148,7 +146,7 @@ struct UbloxTopicDiagnostic { * @param freq_window the number of messages to use for diagnostic statistics */ explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, - double freq_tol, int freq_window) { + double freq_tol, int freq_window, std::shared_ptr updater) { min_freq = freq_min; max_freq = freq_max; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, @@ -183,7 +181,8 @@ struct FixDiagnostic { * @param stamp_min the minimum allowed time delay */ explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, - double stamp_min, uint16_t nav_rate, uint16_t meas_rate) { + double stamp_min, uint16_t nav_rate, uint16_t meas_rate, + std::shared_ptr updater) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz min_freq = target_freq; max_freq = target_freq; @@ -626,6 +625,9 @@ class UbloxNode final { //! The ROS frame ID of this device std::string frame_id_; + + //! ROS diagnostic updater + std::shared_ptr updater_; }; /** @@ -635,6 +637,8 @@ class UbloxNode final { */ class UbloxFirmware : public virtual ComponentInterface { public: + explicit UbloxFirmware(std::shared_ptr updater); + /** * @brief Add the fix diagnostics to the updater. */ @@ -646,6 +650,8 @@ class UbloxFirmware : public virtual ComponentInterface { */ virtual void fixDiagnostic( diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + + std::shared_ptr updater_; }; /** @@ -653,7 +659,7 @@ class UbloxFirmware : public virtual ComponentInterface { */ class UbloxFirmware6 final : public UbloxFirmware { public: - explicit UbloxFirmware6(const std::string & frame_id); + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater); /** * @brief Sets the fix status service type to GPS. @@ -743,7 +749,8 @@ class UbloxFirmware6 final : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: - explicit UbloxFirmware7Plus(const std::string & frame_id) : frame_id_(frame_id) { + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater) + : UbloxFirmware(updater), frame_id_(frame_id) { // NavPVT publisher nav_pvt_pub_ = nh->advertise("navpvt", kROSQueueSize); @@ -847,7 +854,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { // last_nav_pvt_ = m; freq_diag->diagnostic->tick(fix.header.stamp); - updater->update(); + updater_->update(); } protected: @@ -925,7 +932,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - explicit UbloxFirmware7(const std::string & frame_id) : UbloxFirmware7Plus(frame_id) { + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater) + : UbloxFirmware7Plus(frame_id, updater) { nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); } @@ -967,8 +975,9 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { */ class UbloxFirmware8 : public UbloxFirmware7Plus { public: - explicit UbloxFirmware8(const std::string & frame_id) : UbloxFirmware7Plus(frame_id) { - nav_sat_pub_ = nh->advertise("navsate", kROSQueueSize); + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater) + : UbloxFirmware7Plus(frame_id, updater) { + nav_sat_pub_ = nh->advertise("navstate", kROSQueueSize); mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); } @@ -1025,7 +1034,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ class UbloxFirmware9 final : public UbloxFirmware8 { public: - explicit UbloxFirmware9(const std::string & frame_id); + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater); }; /** @@ -1036,7 +1045,7 @@ class RawDataProduct final : public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; - explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate); + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater); /** * @brief Does nothing since there are no Raw Data product specific settings. @@ -1072,6 +1081,7 @@ class RawDataProduct final : public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; + std::shared_ptr updater_; }; /** @@ -1080,7 +1090,7 @@ class RawDataProduct final : public virtual ComponentInterface { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater); /** * @brief Get the ADR/UDR parameters. @@ -1136,6 +1146,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { uint16_t meas_rate_; std::string frame_id_; + std::shared_ptr updater_; }; /** @@ -1144,7 +1155,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1254,6 +1265,7 @@ class HpgRefProduct: public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; bool config_on_startup_flag_; + std::shared_ptr updater_; }; /** @@ -1271,7 +1283,7 @@ class HpgRovProduct final : public virtual ComponentInterface { //! Diagnostic updater: RTCM topic frequency window [num messages] constexpr static int kRtcmFreqWindow = 25; - explicit HpgRovProduct(uint16_t nav_rate); + explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater); /** * @brief Get the ROS parameters specific to the Rover configuration. @@ -1328,11 +1340,12 @@ class HpgRovProduct final : public virtual ComponentInterface { ros::Publisher nav_rel_pos_ned_pub_; uint16_t nav_rate_; + std::shared_ptr updater_; }; class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. @@ -1365,7 +1378,7 @@ class HpPosRecProduct final : public virtual HpgRefProduct { */ class TimProduct final : public virtual ComponentInterface { public: - explicit TimProduct(const std::string & frame_id); + explicit TimProduct(const std::string & frame_id, std::shared_ptr updater); /** * @brief Get the Time Sync parameters. @@ -1407,6 +1420,7 @@ class TimProduct final : public virtual ComponentInterface { ros::Publisher rxm_raw_pub_; std::string frame_id_; + std::shared_ptr updater_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f7661477..f45c8de9 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -115,28 +116,33 @@ UbloxNode::UbloxNode() { int debug; nh->param("debug", debug, 1); gps = std::make_shared(debug); + nav_status_pub_ = nh->advertise("navstatus", kROSQueueSize); nav_posecef_pub_ = nh->advertise("navposecef", kROSQueueSize); nav_clock_pub_ = nh->advertise("navclock", kROSQueueSize); aid_alm_pub_ = nh->advertise("aidalm", kROSQueueSize); aid_eph_pub_ = nh->advertise("aideph", kROSQueueSize); aid_hui_pub_ = nh->advertise("aidhui", kROSQueueSize); + + updater_ = std::make_shared(); + updater_->setHardwareID("ublox"); + initialize(); } void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_)); + components_.push_back(std::make_shared(frame_id_, updater_)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_)); + components_.push_back(std::make_shared(frame_id_, updater_)); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_)); + components_.push_back(std::make_shared(frame_id_, updater_)); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_)); + components_.push_back(std::make_shared(frame_id_, updater_)); ublox_version = 9; } @@ -147,16 +153,16 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, updater_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { - components_.push_back(std::make_shared(nav_rate_)); + components_.push_back(std::make_shared(nav_rate_, updater_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_, updater_)); } else if (product_category.compare("TIM") == 0) { - components_.push_back(std::make_shared(frame_id_)); + components_.push_back(std::make_shared(frame_id_, updater_)); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_)); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -396,12 +402,9 @@ void UbloxNode::initializeRosDiagnostics() { nh->setParam("diagnostic_period", kDiagnosticPeriod); } - updater = std::make_shared(); - updater->setHardwareID("ublox"); - // configure diagnostic updater for frequency freq_diag = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_); + kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } @@ -626,7 +629,7 @@ void UbloxNode::initialize() { processMonVer(); if (protocol_version_ <= 14) { if (nh->param("raw_data", false)) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_)); } } // Must set firmware & hardware params before initializing diagnostics @@ -663,15 +666,20 @@ void UbloxNode::shutdown() { // // U-Blox Firmware (all versions) // +UbloxFirmware::UbloxFirmware(std::shared_ptr updater) : updater_(updater) +{ +} + void UbloxFirmware::initializeRosDiagnostics() { - updater->add("fix", this, &UbloxFirmware::fixDiagnostic); - updater->force_update(); + updater_->add("fix", this, &UbloxFirmware::fixDiagnostic); + updater_->force_update(); } // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id) : frame_id_(frame_id) +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater) + : UbloxFirmware(updater), frame_id_(frame_id) { nav_pos_llh_pub_ = nh->advertise("navposllh", kROSQueueSize); @@ -864,7 +872,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { last_nav_pos_ = m; // update diagnostics freq_diag->diagnostic->tick(fix_.header.stamp); - updater->update(); + updater_->update(); } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { @@ -1354,14 +1362,15 @@ void UbloxFirmware8::subscribe() { } } -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id) : UbloxFirmware8(frame_id) +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater) : UbloxFirmware8(frame_id, updater) { } // // Raw Data Products // -RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate) : nav_rate_(nav_rate), meas_rate_(meas_rate) { +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater) { rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); rxm_eph_pub_ = nh->advertise("rxmeph", kROSQueueSize); @@ -1404,27 +1413,27 @@ void RawDataProduct::subscribe() { void RawDataProduct::initializeRosDiagnostics() { if (enabled["rxm_raw"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } if (enabled["rxm_sfrb"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } if (enabled["rxm_eph"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } if (enabled["rxm_alm"]) { freq_diagnostics_.push_back(std::make_shared( - "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_)); + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } } // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id) - : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater) + : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater) { imu_pub_ = nh->advertise("imu_meas", kROSQueueSize); @@ -1595,14 +1604,14 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { } } - updater->force_update(); + updater_->force_update(); } // // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag) - : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater) + : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag), updater_(updater) { navsvin_pub_ = nh->advertise("navsvin", kROSQueueSize); @@ -1744,7 +1753,7 @@ void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { setTimeMode(); } - updater->update(); + updater_->update(); } bool HpgRefProduct::setTimeMode() { @@ -1766,8 +1775,8 @@ bool HpgRefProduct::setTimeMode() { } void HpgRefProduct::initializeRosDiagnostics() { - updater->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); - updater->force_update(); + updater_->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); + updater_->force_update(); } void HpgRefProduct::tmode3Diagnostics( @@ -1815,7 +1824,8 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // -HpgRovProduct::HpgRovProduct(uint16_t nav_rate) : nav_rate_(nav_rate) +HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater) + : nav_rate_(nav_rate), updater_(updater) { nav_rel_pos_ned_pub_ = nh->advertise("navrelposned", kROSQueueSize); @@ -1846,10 +1856,10 @@ void HpgRovProduct::subscribe() { void HpgRovProduct::initializeRosDiagnostics() { freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow); - updater->add("Carrier Phase Solution", this, + kRtcmFreqTol, kRtcmFreqWindow, updater_); + updater_->add("Carrier Phase Solution", this, &HpgRovProduct::carrierPhaseDiagnostics); - updater->force_update(); + updater_->force_update(); } void HpgRovProduct::carrierPhaseDiagnostics( @@ -1892,14 +1902,14 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { } last_rel_pos_ = m; - updater->update(); + updater_->update(); } // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id) - : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag), frame_id_(frame_id) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater) + : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag, updater), frame_id_(frame_id) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); @@ -1947,13 +1957,13 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { } last_rel_pos_ = m; - updater->update(); + updater_->update(); } // // U-Blox Time Sync Products, partially implemented. // -TimProduct::TimProduct(const std::string & frame_id) : frame_id_(frame_id) +TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater) : frame_id_(frame_id), updater_(updater) { timtm2_pub_ = nh->advertise("timtm2", kROSQueueSize); @@ -2027,11 +2037,11 @@ void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { interrupt_time_pub_.publish(t_ref_); } - updater->force_update(); + updater_->force_update(); } void TimProduct::initializeRosDiagnostics() { - updater->force_update(); + updater_->force_update(); } } // namespace ublox_node From c97208b2c27e85af365fa36ad4e32cb7dfca3af4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:38:46 +0000 Subject: [PATCH 036/152] Make freq_diag a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 26 ++++++++++++++------------ ublox_gps/src/node.cpp | 24 +++++++++++++----------- 2 files changed, 27 insertions(+), 23 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 2de0e586..82d012d6 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -204,9 +204,6 @@ struct FixDiagnostic { double max_freq; }; -//! fix frequency diagnostic updater -std::shared_ptr freq_diag; - /** * @brief Determine dynamic model from human-readable string. * @param model One of the following (case-insensitive): @@ -628,6 +625,9 @@ class UbloxNode final { //! ROS diagnostic updater std::shared_ptr updater_; + + //! fix frequency diagnostic updater + std::shared_ptr freq_diag_; }; /** @@ -659,7 +659,7 @@ class UbloxFirmware : public virtual ComponentInterface { */ class UbloxFirmware6 final : public UbloxFirmware { public: - explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater); + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag); /** * @brief Sets the fix status service type to GPS. @@ -735,6 +735,7 @@ class UbloxFirmware6 final : public UbloxFirmware { ros::Publisher mon_hw_pub_; std::string frame_id_; + std::shared_ptr freq_diag_; }; /** @@ -749,8 +750,8 @@ class UbloxFirmware6 final : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: - explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater) - : UbloxFirmware(updater), frame_id_(frame_id) { + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) + : UbloxFirmware(updater), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher nav_pvt_pub_ = nh->advertise("navpvt", kROSQueueSize); @@ -853,7 +854,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { // Update diagnostics // last_nav_pvt_ = m; - freq_diag->diagnostic->tick(fix.header.stamp); + freq_diag_->diagnostic->tick(fix.header.stamp); updater_->update(); } @@ -925,6 +926,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { ros::Publisher vel_pub_; std::string frame_id_; + std::shared_ptr freq_diag_; }; /** @@ -932,8 +934,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater) - : UbloxFirmware7Plus(frame_id, updater) { + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) + : UbloxFirmware7Plus(frame_id, updater, freq_diag) { nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); } @@ -975,8 +977,8 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { */ class UbloxFirmware8 : public UbloxFirmware7Plus { public: - explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater) - : UbloxFirmware7Plus(frame_id, updater) { + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) + : UbloxFirmware7Plus(frame_id, updater, freq_diag) { nav_sat_pub_ = nh->advertise("navstate", kROSQueueSize); mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); @@ -1034,7 +1036,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ class UbloxFirmware9 final : public UbloxFirmware8 { public: - explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater); + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag); }; /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f45c8de9..cffd73e6 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -127,22 +127,26 @@ UbloxNode::UbloxNode() { updater_ = std::make_shared(); updater_->setHardwareID("ublox"); + // configure diagnostic updater for frequency + freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, + kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); + initialize(); } void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_, updater_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_, updater_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_, updater_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_, updater_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); ublox_version = 9; } @@ -402,9 +406,6 @@ void UbloxNode::initializeRosDiagnostics() { nh->setParam("diagnostic_period", kDiagnosticPeriod); } - // configure diagnostic updater for frequency - freq_diag = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } @@ -678,8 +679,8 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater) - : UbloxFirmware(updater), frame_id_(frame_id) +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) + : UbloxFirmware(updater), frame_id_(frame_id), freq_diag_(freq_diag) { nav_pos_llh_pub_ = nh->advertise("navposllh", kROSQueueSize); @@ -871,7 +872,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_pub_.publish(fix_); last_nav_pos_ = m; // update diagnostics - freq_diag->diagnostic->tick(fix_.header.stamp); + freq_diag_->diagnostic->tick(fix_.header.stamp); updater_->update(); } @@ -1362,7 +1363,8 @@ void UbloxFirmware8::subscribe() { } } -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater) : UbloxFirmware8(frame_id, updater) +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) + : UbloxFirmware8(frame_id, updater, freq_diag) { } From 8b741711b7817f423203c50d6045d1e87af26c75 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:50:01 +0000 Subject: [PATCH 037/152] Switch to a structure for RTCMs. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 9 +++++---- ublox_gps/include/ublox_gps/node.hpp | 6 ++---- ublox_gps/include/ublox_gps/rtcm.hpp | 15 +++++++++++++++ ublox_gps/src/gps.cpp | 10 +++++----- ublox_gps/src/node.cpp | 12 ++++++++++-- 5 files changed, 37 insertions(+), 15 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/rtcm.hpp diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index b907354a..86751f2d 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -32,10 +32,10 @@ // STL #include #include -#include -#include #include +#include #include +#include // ROS #include // Other u-blox packages @@ -43,6 +43,7 @@ // u-blox gps #include #include +#include /** * @namespace ublox_gps @@ -72,7 +73,7 @@ class Gps final { constexpr static int kWriterSize = 2056; explicit Gps(int debug); - virtual ~Gps(); + ~Gps(); /** * @brief If called, when the node shuts down, it will send a command to @@ -195,7 +196,7 @@ class Gps final { * @param rates the send rates for each RTCM message ID, valid range: [0, 255] * @return true on ACK, false on other conditions. */ - bool configRtcm(std::vector ids, std::vector rates); + bool configRtcm(const std::vector & rtcms); /** * @brief Configure the SBAS settings. diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 82d012d6..bad64352 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -50,6 +50,7 @@ // Ublox GPS includes #include #include +#include #include #include @@ -102,11 +103,8 @@ std::map enabled; //! The fix status service type, set in the Firmware Component //! based on the enabled GNSS int fix_status_service; -//! IDs of RTCM out messages to configure. -std::vector rtcm_ids; -//! Rates of RTCM out messages. Size must be the same as rtcm_ids -std::vector rtcm_rates; +std::vector rtcms; //! Topic diagnostics for u-blox messages struct UbloxTopicDiagnostic { diff --git a/ublox_gps/include/ublox_gps/rtcm.hpp b/ublox_gps/include/ublox_gps/rtcm.hpp new file mode 100644 index 00000000..ee39bc4e --- /dev/null +++ b/ublox_gps/include/ublox_gps/rtcm.hpp @@ -0,0 +1,15 @@ +#ifndef UBLOX_GPS_RTCM_HPP +#define UBLOX_GPS_RTCM_HPP + +namespace ublox_gps { + +struct Rtcm { + //! ID of RTCM out message to configure. + uint8_t id; + //! Rate of RTCM out message. + uint8_t rate; +}; + +} // namespace ublox_node + +#endif diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 514fd3e1..768d3d5f 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -400,11 +400,11 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { return configure(rate); } -bool Gps::configRtcm(std::vector ids, std::vector rates) { - for (size_t i = 0; i < ids.size(); ++i) { - ROS_DEBUG("Setting RTCM %d Rate %u", ids[i], rates[i]); - if (!setRate(ublox_msgs::Class::RTCM, (uint8_t)ids[i], rates[i])) { - ROS_ERROR("Could not set RTCM %d to rate %u", ids[i], rates[i]); +bool Gps::configRtcm(const std::vector & rtcms) { + for (size_t i = 0; i < rtcms.size(); ++i) { + ROS_DEBUG("Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); + if (!setRate(ublox_msgs::Class::RTCM, rtcms[i].id, rtcms[i].rate)) { + ROS_ERROR("Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); return false; } } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index cffd73e6..80e34d1e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -210,6 +210,8 @@ void UbloxNode::getRosParams() { nh->param("rate", rate_, 4.0); // in Hz getRosUint("nav_rate", nav_rate_, 1); // # of measurement rate cycles // RTCM params + std::vector rtcm_ids; + std::vector rtcm_rates; getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting @@ -233,6 +235,12 @@ void UbloxNode::getRosParams() { " must match size of rtcm_rates"); } + rtcms.resize(rtcm_ids.size()); + for (size_t i = 0; i < rtcm_ids.size(); ++i) { + rtcms[i].id = rtcm_ids[i]; + rtcms[i].rate = rtcm_rates[i]; + } + dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); @@ -1676,7 +1684,7 @@ bool HpgRefProduct::configureUblox() { fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); } - if (!gps->configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps->configRtcm(rtcms)) { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; @@ -1769,7 +1777,7 @@ bool HpgRefProduct::setTimeMode() { "navigation rate to ", nav_rate_); } // Enable the RTCM out messages - if (!gps->configRtcm(rtcm_ids, rtcm_rates)) { + if (!gps->configRtcm(rtcms)) { ROS_ERROR("Failed to configure RTCM IDs"); return false; } From f322c4d6f4cdde52e86dcfe8ed0f8d359a42b83b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:56:09 +0000 Subject: [PATCH 038/152] Make rtcms a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 10 ++++++---- ublox_gps/src/node.cpp | 22 +++++++++++----------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index bad64352..fb942bfa 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -104,8 +104,6 @@ std::map enabled; //! based on the enabled GNSS int fix_status_service; -std::vector rtcms; - //! Topic diagnostics for u-blox messages struct UbloxTopicDiagnostic { UbloxTopicDiagnostic() {} @@ -626,6 +624,8 @@ class UbloxNode final { //! fix frequency diagnostic updater std::shared_ptr freq_diag_; + + std::vector rtcms_; }; /** @@ -1155,7 +1155,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater, std::vector rtcms); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1266,6 +1266,8 @@ class HpgRefProduct: public virtual ComponentInterface { uint16_t meas_rate_; bool config_on_startup_flag_; std::shared_ptr updater_; + + std::vector rtcms_; }; /** @@ -1345,7 +1347,7 @@ class HpgRovProduct final : public virtual ComponentInterface { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 80e34d1e..f17441cd 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -157,11 +157,11 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, updater_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, updater_, rtcms_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared(nav_rate_, updater_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_, updater_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_, updater_, rtcms_)); } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared(frame_id_, updater_)); } else if (product_category.compare("ADR") == 0 || @@ -235,10 +235,10 @@ void UbloxNode::getRosParams() { " must match size of rtcm_rates"); } - rtcms.resize(rtcm_ids.size()); + rtcms_.resize(rtcm_ids.size()); for (size_t i = 0; i < rtcm_ids.size(); ++i) { - rtcms[i].id = rtcm_ids[i]; - rtcms[i].rate = rtcm_rates[i]; + rtcms_[i].id = rtcm_ids[i]; + rtcms_[i].rate = rtcm_rates[i]; } dmodel_ = modelFromString(dynamic_model_); @@ -1620,8 +1620,8 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater) - : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag), updater_(updater) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater, std::vector rtcms) + : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag), updater_(updater), rtcms_(rtcms) { navsvin_pub_ = nh->advertise("navsvin", kROSQueueSize); @@ -1684,7 +1684,7 @@ bool HpgRefProduct::configureUblox() { fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); } - if (!gps->configRtcm(rtcms)) { + if (!gps->configRtcm(rtcms_)) { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; @@ -1777,7 +1777,7 @@ bool HpgRefProduct::setTimeMode() { "navigation rate to ", nav_rate_); } // Enable the RTCM out messages - if (!gps->configRtcm(rtcms)) { + if (!gps->configRtcm(rtcms_)) { ROS_ERROR("Failed to configure RTCM IDs"); return false; } @@ -1918,8 +1918,8 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater) - : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag, updater), frame_id_(frame_id) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms) + : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag, updater, rtcms), frame_id_(frame_id) { nav_relposned_pub_ = nh->advertise("navrelposned", kROSQueueSize); From 2984f94e62db9f0743c669f86ab8ba84b5f65ea1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:57:45 +0000 Subject: [PATCH 039/152] Remove kROSQueueSize constant. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 18 ++++---- ublox_gps/src/node.cpp | 66 ++++++++++++++-------------- 2 files changed, 41 insertions(+), 43 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index fb942bfa..ce4c62ba 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -77,8 +77,6 @@ */ namespace ublox_node { -//! Queue size for ROS publishers -constexpr static uint32_t kROSQueueSize = 1; //! Default measurement period for HPG devices constexpr static uint16_t kDefaultMeasPeriod = 250; //! Default subscribe Rate to u-blox messages [Hz] @@ -751,13 +749,13 @@ class UbloxFirmware7Plus : public UbloxFirmware { explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) : UbloxFirmware(updater), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher - nav_pvt_pub_ = nh->advertise("navpvt", kROSQueueSize); + nav_pvt_pub_ = nh->advertise("navpvt", 1); fix_pub_ = - nh->advertise("fix", kROSQueueSize); + nh->advertise("fix", 1); vel_pub_ = nh->advertise("fix_velocity", - kROSQueueSize); + 1); } /** @@ -934,8 +932,8 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) : UbloxFirmware7Plus(frame_id, updater, freq_diag) { - nav_svinfo_pub_ = nh->advertise("navsvinfo", kROSQueueSize); - mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); + nav_svinfo_pub_ = nh->advertise("navsvinfo", 1); + mon_hw_pub_ = nh->advertise("monhw", 1); } /** @@ -977,9 +975,9 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { public: explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) : UbloxFirmware7Plus(frame_id, updater, freq_diag) { - nav_sat_pub_ = nh->advertise("navstate", kROSQueueSize); - mon_hw_pub_ = nh->advertise("monhw", kROSQueueSize); - rxm_rtcm_pub_ = nh->advertise("rxmrtcm", kROSQueueSize); + nav_sat_pub_ = nh->advertise("navstate", 1); + mon_hw_pub_ = nh->advertise("monhw", 1); + rxm_rtcm_pub_ = nh->advertise("rxmrtcm", 1); } /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f17441cd..0b2b33e1 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -117,12 +117,12 @@ UbloxNode::UbloxNode() { nh->param("debug", debug, 1); gps = std::make_shared(debug); - nav_status_pub_ = nh->advertise("navstatus", kROSQueueSize); - nav_posecef_pub_ = nh->advertise("navposecef", kROSQueueSize); - nav_clock_pub_ = nh->advertise("navclock", kROSQueueSize); - aid_alm_pub_ = nh->advertise("aidalm", kROSQueueSize); - aid_eph_pub_ = nh->advertise("aideph", kROSQueueSize); - aid_hui_pub_ = nh->advertise("aidhui", kROSQueueSize); + nav_status_pub_ = nh->advertise("navstatus", 1); + nav_posecef_pub_ = nh->advertise("navposecef", 1); + nav_clock_pub_ = nh->advertise("navclock", 1); + aid_alm_pub_ = nh->advertise("aidalm", 1); + aid_eph_pub_ = nh->advertise("aideph", 1); + aid_hui_pub_ = nh->advertise("aidhui", 1); updater_ = std::make_shared(); updater_->setHardwareID("ublox"); @@ -691,25 +691,25 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptradvertise("navposllh", kROSQueueSize); + nh->advertise("navposllh", 1); fix_pub_ = - nh->advertise("fix", kROSQueueSize); + nh->advertise("fix", 1); nav_vel_ned_pub_ = - nh->advertise("navvelned", kROSQueueSize); + nh->advertise("navvelned", 1); vel_pub_ = nh->advertise("fix_velocity", - kROSQueueSize); + 1); nav_sol_pub_ = - nh->advertise("navsol", kROSQueueSize); + nh->advertise("navsol", 1); nav_svinfo_pub_ = - nh->advertise("navinfo", kROSQueueSize); + nh->advertise("navinfo", 1); mon_hw_pub_ = - nh->advertise("monhw", kROSQueueSize); + nh->advertise("monhw", 1); } void UbloxFirmware6::getRosParams() { @@ -1381,10 +1381,10 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater) : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater) { - rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); - rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); - rxm_eph_pub_ = nh->advertise("rxmeph", kROSQueueSize); - rxm_alm_pub_ = nh->advertise("rxmalm", kROSQueueSize); + rxm_raw_pub_ = nh->advertise("rxmraw", 1); + rxm_sfrb_pub_ = nh->advertise("rxmsfrb", 1); + rxm_eph_pub_ = nh->advertise("rxmeph", 1); + rxm_alm_pub_ = nh->advertise("rxmalm", 1); } void RawDataProduct::subscribe() { @@ -1446,15 +1446,15 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater) { imu_pub_ = - nh->advertise("imu_meas", kROSQueueSize); + nh->advertise("imu_meas", 1); time_ref_pub_ = - nh->advertise("interrupt_time", kROSQueueSize); - nav_att_pub_ = nh->advertise("navatt", kROSQueueSize); - esf_ins_pub_ = nh->advertise("esfins", kROSQueueSize); - esf_meas_pub_ = nh->advertise("esfmeas", kROSQueueSize); - esf_raw_pub_ = nh->advertise("esfraw", kROSQueueSize); - esf_status_pub_ = nh->advertise("esfstatus", kROSQueueSize); - hnr_pvt_pub_ = nh->advertise("hnrpvt", kROSQueueSize); + nh->advertise("interrupt_time", 1); + nav_att_pub_ = nh->advertise("navatt", 1); + esf_ins_pub_ = nh->advertise("esfins", 1); + esf_meas_pub_ = nh->advertise("esfmeas", 1); + esf_raw_pub_ = nh->advertise("esfraw", 1); + esf_status_pub_ = nh->advertise("esfstatus", 1); + hnr_pvt_pub_ = nh->advertise("hnrpvt", 1); } void AdrUdrProduct::getRosParams() { @@ -1624,7 +1624,7 @@ HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_ : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag), updater_(updater), rtcms_(rtcms) { navsvin_pub_ = - nh->advertise("navsvin", kROSQueueSize); + nh->advertise("navsvin", 1); } void HpgRefProduct::getRosParams() { @@ -1838,7 +1838,7 @@ HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptradvertise("navrelposned", kROSQueueSize); + nh->advertise("navrelposned", 1); } void HpgRovProduct::getRosParams() { @@ -1922,10 +1922,10 @@ HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool con : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag, updater, rtcms), frame_id_(frame_id) { nav_relposned_pub_ = - nh->advertise("navrelposned", kROSQueueSize); + nh->advertise("navrelposned", 1); imu_pub_ = - nh->advertise("navheading", kROSQueueSize); + nh->advertise("navheading", 1); } void HpPosRecProduct::subscribe() { @@ -1976,11 +1976,11 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater) : frame_id_(frame_id), updater_(updater) { timtm2_pub_ = - nh->advertise("timtm2", kROSQueueSize); + nh->advertise("timtm2", 1); interrupt_time_pub_ = - nh->advertise("interrupt_time", kROSQueueSize); - rxm_sfrb_pub_ = nh->advertise("rxmsfrb", kROSQueueSize); - rxm_raw_pub_ = nh->advertise("rxmraw", kROSQueueSize); + nh->advertise("interrupt_time", 1); + rxm_sfrb_pub_ = nh->advertise("rxmsfrb", 1); + rxm_raw_pub_ = nh->advertise("rxmraw", 1); } void TimProduct::getRosParams() { From 9286f88f9c21acb45d9d8b00669edd641c74e107 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 22:59:09 +0000 Subject: [PATCH 040/152] Move the kDefaultMeasRate to the class that needs it. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index ce4c62ba..3da6e928 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -77,8 +77,6 @@ */ namespace ublox_node { -//! Default measurement period for HPG devices -constexpr static uint16_t kDefaultMeasPeriod = 250; //! Default subscribe Rate to u-blox messages [Hz] constexpr static uint32_t kSubscribeRate = 1; //! Subscribe Rate for u-blox SV Info messages @@ -1153,6 +1151,9 @@ class AdrUdrProduct final : public virtual ComponentInterface { */ class HpgRefProduct: public virtual ComponentInterface { public: + //! Default measurement period for HPG devices + constexpr static uint16_t kDefaultMeasPeriod = 250; + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater, std::vector rtcms); /** From 07638b70714f2d5b1890535ec9eb03178a46c2d0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 23:01:31 +0000 Subject: [PATCH 041/152] Remove kSubscribeRate. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 2 - ublox_gps/src/node.cpp | 64 ++++++++++++++-------------- 2 files changed, 32 insertions(+), 34 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 3da6e928..46af06e3 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -77,8 +77,6 @@ */ namespace ublox_node { -//! Default subscribe Rate to u-blox messages [Hz] -constexpr static uint32_t kSubscribeRate = 1; //! Subscribe Rate for u-blox SV Info messages constexpr static uint32_t kNavSvInfoSubscribeRate = 20; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 0b2b33e1..df479eb4 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -329,19 +329,19 @@ void UbloxNode::subscribe() { nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_status_pub_), kSubscribeRate); + publish, std::placeholders::_1, nav_status_pub_), 1); } nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_posecef_pub_), kSubscribeRate); + publish, std::placeholders::_1, nav_posecef_pub_), 1); } nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_clock_pub_), kSubscribeRate); + publish, std::placeholders::_1, nav_clock_pub_), 1); } // INF messages @@ -389,19 +389,19 @@ void UbloxNode::subscribe() { nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, aid_alm_pub_), kSubscribeRate); + publish, std::placeholders::_1, aid_alm_pub_), 1); } nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, aid_eph_pub_), kSubscribeRate); + publish, std::placeholders::_1, aid_eph_pub_), 1); } nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, aid_hui_pub_), kSubscribeRate); + publish, std::placeholders::_1, aid_hui_pub_), 1); } for (size_t i = 0; i < components_.size(); i++) { @@ -777,13 +777,13 @@ void UbloxFirmware6::subscribe() { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH gps->subscribe(std::bind( - &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), kSubscribeRate); + &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), 1); gps->subscribe(std::bind( // Subscribe to Nav SOL - &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), kSubscribeRate); + &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), 1); // Subscribe to Nav VELNED gps->subscribe(std::bind( - &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), kSubscribeRate); + &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); @@ -797,7 +797,7 @@ void UbloxFirmware6::subscribe() { nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), 1); } } @@ -1100,7 +1100,7 @@ void UbloxFirmware7::subscribe() { // from this) gps->subscribe(std::bind( &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), - kSubscribeRate); + 1); // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); @@ -1114,7 +1114,7 @@ void UbloxFirmware7::subscribe() { nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), 1); } } @@ -1347,7 +1347,7 @@ void UbloxFirmware8::subscribe() { nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT gps->subscribe( - std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), kSubscribeRate); + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); @@ -1360,14 +1360,14 @@ void UbloxFirmware8::subscribe() { nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), kSubscribeRate); + publish, std::placeholders::_1, mon_hw_pub_), 1); } // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_rtcm_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_rtcm_pub_), 1); } } @@ -1395,28 +1395,28 @@ void RawDataProduct::subscribe() { nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_raw_pub_), 1); } // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_sfrb_pub_), 1); } // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_eph_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_eph_pub_), 1); } // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_alm_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_alm_pub_), 1); } } @@ -1481,45 +1481,45 @@ void AdrUdrProduct::subscribe() { nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_att_pub_), kSubscribeRate); + publish, std::placeholders::_1, nav_att_pub_), 1); } // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_ins_pub_), kSubscribeRate); + publish, std::placeholders::_1, esf_ins_pub_), 1); } // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_meas_pub_), kSubscribeRate); + publish, std::placeholders::_1, esf_meas_pub_), 1); // also publish sensor_msgs::Imu gps->subscribe(std::bind( - &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), kSubscribeRate); + &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); } // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_raw_pub_), kSubscribeRate); + publish, std::placeholders::_1, esf_raw_pub_), 1); } // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_status_pub_), kSubscribeRate); + publish, std::placeholders::_1, esf_status_pub_), 1); } // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, hnr_pvt_pub_), kSubscribeRate); + publish, std::placeholders::_1, hnr_pvt_pub_), 1); } } @@ -1749,7 +1749,7 @@ void HpgRefProduct::subscribe() { nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In gps->subscribe(std::bind( - &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), kSubscribeRate); + &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); } void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { @@ -1860,7 +1860,7 @@ void HpgRovProduct::subscribe() { nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( - &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); + &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); } void HpgRovProduct::initializeRosDiagnostics() { @@ -1933,7 +1933,7 @@ void HpPosRecProduct::subscribe() { nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), kSubscribeRate); + &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); // Whether to publish the Heading info from Nav Relative Position NED nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); @@ -2007,7 +2007,7 @@ void TimProduct::subscribe() { nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); gps->subscribe(std::bind( - &TimProduct::callbackTimTM2, this, std::placeholders::_1), kSubscribeRate); + &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); @@ -2015,14 +2015,14 @@ void TimProduct::subscribe() { nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_sfrb_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_sfrb_pub_), 1); } // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_raw_pub_), kSubscribeRate); + publish, std::placeholders::_1, rxm_raw_pub_), 1); } } From 72f7013bdfb8559f376ccd756fa349abc630e74d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 23:03:01 +0000 Subject: [PATCH 042/152] Move kNavSvInfoSubscribeRate into the base class that uses it. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 46af06e3..8ee0c745 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -77,9 +77,6 @@ */ namespace ublox_node { -//! Subscribe Rate for u-blox SV Info messages -constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - // ROS objects //! Node Handle for GPS node std::shared_ptr nh; @@ -629,6 +626,9 @@ class UbloxNode final { */ class UbloxFirmware : public virtual ComponentInterface { public: + //! Subscribe Rate for u-blox SV Info messages + constexpr static uint32_t kNavSvInfoSubscribeRate = 20; + explicit UbloxFirmware(std::shared_ptr updater); /** From e8b75aa1948aab52c8c05d512b7fd5a17118d686 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 23:31:26 +0000 Subject: [PATCH 043/152] Make gnss a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/gnss.hpp | 23 +++++++++++ ublox_gps/include/ublox_gps/node.hpp | 33 +++++++--------- ublox_gps/src/gnss.cpp | 25 ++++++++++++ ublox_gps/src/node.cpp | 57 +++++++++++++++------------- 5 files changed, 92 insertions(+), 48 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/gnss.hpp create mode 100644 ublox_gps/src/gnss.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 98b1b258..5dd657e7 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/gnss.hpp b/ublox_gps/include/ublox_gps/gnss.hpp new file mode 100644 index 00000000..0d3e61ad --- /dev/null +++ b/ublox_gps/include/ublox_gps/gnss.hpp @@ -0,0 +1,23 @@ +#ifndef UBLOX_GPS_GNSS_HPP +#define UBLOX_GPS_GNSS_HPP + +#include +#include + +namespace ublox_node { + +class Gnss final { +public: + Gnss(); + ~Gnss(); + + void add(const std::string & gnss); + bool isSupported(const std::string & gnss); + +private: + std::set supported_; +}; + +} + +#endif diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 8ee0c745..633d87d4 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -83,8 +83,6 @@ std::shared_ptr nh; //! Handles communication with the U-Blox Device std::shared_ptr gps; -//! Which GNSS are supported by the device -std::set supported; //! Whether or not to publish the given ublox message /*! * key is the message name (all lowercase) without firmware version numbers @@ -401,15 +399,6 @@ void publish(const MessageT& m, ros::Publisher & publisher) { publisher.publish(m); } -/** - * @param gnss The string representing the GNSS. Refer MonVER message protocol. - * i.e. GPS, GLO, GAL, BDS, QZSS, SBAS, IMES - * @return true if the device supports the given GNSS - */ -bool supportsGnss(const std::string & gnss) { - return supported.count(gnss) > 0; -} - /** * @brief This class represents u-blox ROS node for *all* firmware and product * versions. @@ -617,6 +606,9 @@ class UbloxNode final { std::shared_ptr freq_diag_; std::vector rtcms_; + + //! Which GNSS are supported by the device + std::shared_ptr gnss_; }; /** @@ -629,7 +621,7 @@ class UbloxFirmware : public virtual ComponentInterface { //! Subscribe Rate for u-blox SV Info messages constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - explicit UbloxFirmware(std::shared_ptr updater); + explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss); /** * @brief Add the fix diagnostics to the updater. @@ -644,6 +636,7 @@ class UbloxFirmware : public virtual ComponentInterface { diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; std::shared_ptr updater_; + std::shared_ptr gnss_; }; /** @@ -651,7 +644,7 @@ class UbloxFirmware : public virtual ComponentInterface { */ class UbloxFirmware6 final : public UbloxFirmware { public: - explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag); + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss); /** * @brief Sets the fix status service type to GPS. @@ -742,8 +735,8 @@ class UbloxFirmware6 final : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: - explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) - : UbloxFirmware(updater), frame_id_(frame_id), freq_diag_(freq_diag) { + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + : UbloxFirmware(updater, gnss), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher nav_pvt_pub_ = nh->advertise("navpvt", 1); @@ -926,8 +919,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) - : UbloxFirmware7Plus(frame_id, updater, freq_diag) { + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss) { nav_svinfo_pub_ = nh->advertise("navsvinfo", 1); mon_hw_pub_ = nh->advertise("monhw", 1); } @@ -969,8 +962,8 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { */ class UbloxFirmware8 : public UbloxFirmware7Plus { public: - explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) - : UbloxFirmware7Plus(frame_id, updater, freq_diag) { + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss) { nav_sat_pub_ = nh->advertise("navstate", 1); mon_hw_pub_ = nh->advertise("monhw", 1); rxm_rtcm_pub_ = nh->advertise("rxmrtcm", 1); @@ -1028,7 +1021,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ class UbloxFirmware9 final : public UbloxFirmware8 { public: - explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag); + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss); }; /** diff --git a/ublox_gps/src/gnss.cpp b/ublox_gps/src/gnss.cpp new file mode 100644 index 00000000..35e15c73 --- /dev/null +++ b/ublox_gps/src/gnss.cpp @@ -0,0 +1,25 @@ +#include + +#include + +namespace ublox_node { + +Gnss::Gnss() +{ +} + +Gnss::~Gnss() +{ +} + +void Gnss::add(const std::string & gnss) +{ + supported_.insert(gnss); +} + +bool Gnss::isSupported(const std::string & gnss) +{ + return supported_.count(gnss) > 0; +} + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index df479eb4..c03c5f7b 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -39,7 +39,8 @@ #include #include -#include "ublox_gps/node.hpp" +#include +#include namespace ublox_node { @@ -117,6 +118,8 @@ UbloxNode::UbloxNode() { nh->param("debug", debug, 1); gps = std::make_shared(debug); + gnss_ = std::make_shared(); + nav_status_pub_ = nh->advertise("navstatus", 1); nav_posecef_pub_ = nh->advertise("navposecef", 1); nav_clock_pub_ = nh->advertise("navclock", 1); @@ -137,16 +140,16 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); ublox_version = 9; } @@ -460,7 +463,7 @@ void UbloxNode::processMonVer() { strs = stringSplit(extension[extension.size() - 1], ";"); } for (size_t i = 0; i < strs.size(); i++) { - supported.insert(strs[i]); + gnss_->add(strs[i]); } } else { for (std::size_t i = 0; i < extension.size(); ++i) { @@ -483,7 +486,7 @@ void UbloxNode::processMonVer() { if (i >= extension.size() - 2) { strs = stringSplit(extension[i], ";"); for (size_t i = 0; i < strs.size(); i++) { - supported.insert(strs[i]); + gnss_->add(strs[i]); } } } @@ -521,7 +524,7 @@ bool UbloxNode::configureUblox() { throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) - if (supportsGnss("SBAS")) { + if (gnss_->isSupported("SBAS")) { if (!gps->configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + @@ -675,7 +678,7 @@ void UbloxNode::shutdown() { // // U-Blox Firmware (all versions) // -UbloxFirmware::UbloxFirmware(std::shared_ptr updater) : updater_(updater) +UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss) : updater_(updater), gnss_(gnss) { } @@ -687,8 +690,8 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) - : UbloxFirmware(updater), frame_id_(frame_id), freq_diag_(freq_diag) +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + : UbloxFirmware(updater, gnss), frame_id_(frame_id), freq_diag_(freq_diag) { nav_pos_llh_pub_ = nh->advertise("navposllh", 1); @@ -936,17 +939,17 @@ void UbloxFirmware7::getRosParams() { ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); nh->param("gnss/sbas", enable_sbas_, false); - if (enable_gps_ && !supportsGnss("GPS")) { + if (enable_gps_ && !gnss_->isSupported("GPS")) { ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); } - if (enable_glonass_ && !supportsGnss("GLO")) { + if (enable_glonass_ && !gnss_->isSupported("GLO")) { ROS_WARN("gnss/glonass is true, but GLONASS is not %s", "supported by this device"); } - if (enable_qzss_ && !supportsGnss("QZSS")) { + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (enable_sbas_ && !supportsGnss("SBAS")) { + if (enable_sbas_ && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1042,7 +1045,7 @@ bool UbloxFirmware7::configureUblox() { cfgGNSSWrite.msg_ver = 0; // configure GLONASS - if (supportsGnss("GLO")) { + if (gnss_->isSupported("GLO")) { ublox_msgs::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_GLONASS; block.res_trk_ch = block.RES_TRK_CH_GLONASS; @@ -1056,7 +1059,7 @@ bool UbloxFirmware7::configureUblox() { } } - if (supportsGnss("QZSS")) { + if (gnss_->isSupported("QZSS")) { // configure QZSS ublox_msgs::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_QZSS; @@ -1071,7 +1074,7 @@ bool UbloxFirmware7::configureUblox() { } } - if (supportsGnss("SBAS")) { + if (gnss_->isSupported("SBAS")) { // configure SBAS ublox_msgs::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_SBAS; @@ -1138,30 +1141,30 @@ void UbloxFirmware8::getRosParams() { getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); - if (enable_gps_ && !supportsGnss("GPS")) { + if (enable_gps_ && !gnss_->isSupported("GPS")) { ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", "this device"); } - if (enable_glonass_ && !supportsGnss("GLO")) { + if (enable_glonass_ && !gnss_->isSupported("GLO")) { ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", "this device"); } - if (enable_galileo_ && !supportsGnss("GAL")) { + if (enable_galileo_ && !gnss_->isSupported("GAL")) { ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", "by this device"); } - if (enable_beidou_ && !supportsGnss("BDS")) { + if (enable_beidou_ && !gnss_->isSupported("BDS")) { ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", "by this device"); } - if (enable_imes_ && !supportsGnss("IMES")) { + if (enable_imes_ && !gnss_->isSupported("IMES")) { ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", "this device"); } - if (enable_qzss_ && !supportsGnss("QZSS")) { + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (enable_sbas_ && !supportsGnss("SBAS")) { + if (enable_sbas_ && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1371,8 +1374,8 @@ void UbloxFirmware8::subscribe() { } } -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag) - : UbloxFirmware8(frame_id, updater, freq_diag) +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + : UbloxFirmware8(frame_id, updater, freq_diag, gnss) { } From 9a1aca24c865a5730468fa80610613ae1308a8ff Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 23:34:32 +0000 Subject: [PATCH 044/152] Move UbloxTopicDiagnostic to its own file. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 57 +--------------- .../ublox_gps/ublox_topic_diagnostic.hpp | 67 +++++++++++++++++++ ublox_gps/src/node.cpp | 1 + 3 files changed, 69 insertions(+), 56 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 633d87d4..3814fb33 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -53,6 +53,7 @@ #include #include #include +#include // This file also declares UbloxNode which implements ComponentInterface and is // the main class and ros node. it implements functionality which applies to @@ -93,62 +94,6 @@ std::map enabled; //! based on the enabled GNSS int fix_status_service; -//! Topic diagnostics for u-blox messages -struct UbloxTopicDiagnostic { - UbloxTopicDiagnostic() {} - - // Must not copy this struct (would confuse FrequencyStatusParam pointers) - UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; - - /** - * @brief Add a topic diagnostic to the diagnostic updater for - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - */ - explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, - uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz - min_freq = target_freq; - max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = std::make_shared(topic, - *updater, - freq_param); - } - - /** - * @brief Add a topic diagnostic to the diagnostic updater for - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_min the minimum acceptable frequency for the topic - * @param freq_max the maximum acceptable frequency for the topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - */ - explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, - double freq_tol, int freq_window, std::shared_ptr updater) { - min_freq = freq_min; - max_freq = freq_max; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - diagnostic = std::make_shared(topic, - *updater, - freq_param); - } - - //! Topic frequency diagnostic updater - std::shared_ptr diagnostic; - //! Minimum allow frequency of topic - double min_freq; - //! Maximum allow frequency of topic - double max_freq; -}; - //! Topic diagnostics for fix / fix_velocity messages struct FixDiagnostic { FixDiagnostic() {} diff --git a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp new file mode 100644 index 00000000..59b49d95 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp @@ -0,0 +1,67 @@ +#ifndef UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP +#define UBLOX_GPS_UBLOX_TOPIC_DIAGNOSTIC_HPP + +#include +#include + +namespace ublox_node { + +//! Topic diagnostics for u-blox messages +struct UbloxTopicDiagnostic { + UbloxTopicDiagnostic() {} + + // Must not copy this struct (would confuse FrequencyStatusParam pointers) + UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; + + /** + * @brief Add a topic diagnostic to the diagnostic updater for + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + */ + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_tol, int freq_window, + uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + min_freq = target_freq; + max_freq = target_freq; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + diagnostic = std::make_shared(topic, + *updater, + freq_param); + } + + /** + * @brief Add a topic diagnostic to the diagnostic updater for + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_min the minimum acceptable frequency for the topic + * @param freq_max the maximum acceptable frequency for the topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + */ + explicit UbloxTopicDiagnostic(const std::string & topic, double freq_min, double freq_max, + double freq_tol, int freq_window, std::shared_ptr updater) { + min_freq = freq_min; + max_freq = freq_max; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + diagnostic = std::make_shared(topic, + *updater, + freq_param); + } + + //! Topic frequency diagnostic updater + std::shared_ptr diagnostic; + //! Minimum allow frequency of topic + double min_freq; + //! Maximum allow frequency of topic + double max_freq; +}; + +} + +#endif diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index c03c5f7b..f1f6e88d 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace ublox_node { From c6a81b15456da38acdd50051b3d2af56316c14ad Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Nov 2019 23:38:36 +0000 Subject: [PATCH 045/152] Move FixDiagnostic class to its own file. Signed-off-by: Chris Lalancette --- .../include/ublox_gps/fix_diagnostic.hpp | 54 +++++++++++++++++++ ublox_gps/include/ublox_gps/node.hpp | 41 +------------- ublox_gps/src/node.cpp | 1 + 3 files changed, 56 insertions(+), 40 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/fix_diagnostic.hpp diff --git a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp new file mode 100644 index 00000000..2a8c0f61 --- /dev/null +++ b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp @@ -0,0 +1,54 @@ +#ifndef UBLOX_GPS_FIX_DIAGNOSTIC_HPP +#define UBLOX_GPS_FIX_DIAGNOSTIC_HPP + +#include +#include + +#include +#include + +namespace ublox_node { + +//! Topic diagnostics for fix / fix_velocity messages +struct FixDiagnostic { + FixDiagnostic() {} + + // Must not copy this struct (would confuse FrequencyStatusParam pointers) + FixDiagnostic(const FixDiagnostic&) = delete; + + /** + * @brief Add a topic diagnostic to the diagnostic updater for fix topics. + * + * @details The minimum and maximum frequency are equal to the nav rate in Hz. + * @param name the ROS topic + * @param freq_tol the tolerance [%] for the topic frequency + * @param freq_window the number of messages to use for diagnostic statistics + * @param stamp_min the minimum allowed time delay + */ + explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, + double stamp_min, uint16_t nav_rate, uint16_t meas_rate, + std::shared_ptr updater) { + const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + min_freq = target_freq; + max_freq = target_freq; + diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, + freq_tol, freq_window); + double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); + diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); + diagnostic = std::make_shared(name, + *updater, + freq_param, + time_param); + } + + //! Topic frequency diagnostic updater + std::shared_ptr diagnostic; + //! Minimum allow frequency of topic + double min_freq; + //! Maximum allow frequency of topic + double max_freq; +}; + +} + +#endif diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 3814fb33..f83caf0a 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -49,6 +49,7 @@ #include // Ublox GPS includes #include +#include #include #include #include @@ -94,46 +95,6 @@ std::map enabled; //! based on the enabled GNSS int fix_status_service; -//! Topic diagnostics for fix / fix_velocity messages -struct FixDiagnostic { - FixDiagnostic() {} - - // Must not copy this struct (would confuse FrequencyStatusParam pointers) - FixDiagnostic(const FixDiagnostic&) = delete; - - /** - * @brief Add a topic diagnostic to the diagnostic updater for fix topics. - * - * @details The minimum and maximum frequency are equal to the nav rate in Hz. - * @param name the ROS topic - * @param freq_tol the tolerance [%] for the topic frequency - * @param freq_window the number of messages to use for diagnostic statistics - * @param stamp_min the minimum allowed time delay - */ - explicit FixDiagnostic(const std::string & name, double freq_tol, int freq_window, - double stamp_min, uint16_t nav_rate, uint16_t meas_rate, - std::shared_ptr updater) { - const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz - min_freq = target_freq; - max_freq = target_freq; - diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, - freq_tol, freq_window); - double stamp_max = meas_rate * 1e-3 * (1 + freq_tol); - diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max); - diagnostic = std::make_shared(name, - *updater, - freq_param, - time_param); - } - - //! Topic frequency diagnostic updater - std::shared_ptr diagnostic; - //! Minimum allow frequency of topic - double min_freq; - //! Maximum allow frequency of topic - double max_freq; -}; - /** * @brief Determine dynamic model from human-readable string. * @param model One of the following (case-insensitive): diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f1f6e88d..a60cd9b5 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -38,6 +38,7 @@ #include +#include #include #include #include From f37c9eaef56e5cac38fc569cad827208c6a2842c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 14:13:33 +0000 Subject: [PATCH 046/152] Minor code improvements. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 8 ++++++-- ublox_gps/src/node.cpp | 7 +++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index f83caf0a..1fa2b6cf 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -31,8 +31,11 @@ #define UBLOX_GPS_NODE_HPP // STL +#include #include -#include +#include +#include +#include #include // ROS includes #include @@ -914,6 +917,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { ublox_msgs::CfgNMEA cfg_nmea_; //! Whether to clear the flash memory during configuration bool clear_bbr_; + bool save_on_shutdown_; ros::Publisher nav_sat_pub_; ros::Publisher mon_hw_pub_; @@ -1094,7 +1098,7 @@ class HpgRefProduct: public virtual ComponentInterface { * configured RTCM messages. Publish received Nav SVIN messages if enabled. * @param m the message to process */ - void callbackNavSvIn(ublox_msgs::NavSVIN m); + void callbackNavSvIn(const ublox_msgs::NavSVIN& m); protected: /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index a60cd9b5..f7284eb1 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -1129,7 +1129,7 @@ void UbloxFirmware7::subscribe() { void UbloxFirmware8::getRosParams() { // UPD SOS configuration nh->param("clear_bbr", clear_bbr_, false); - gps->setSaveOnShutdown(nh->param("save_on_shutdown", false)); + nh->param("save_on_shutdown", save_on_shutdown_, false); // GNSS enable/disable nh->param("gnss/gps", enable_gps_, true); @@ -1257,6 +1257,9 @@ bool UbloxFirmware8::configureUblox() { ROS_ERROR("u-blox failed to clear flash memory"); } } + + gps->setSaveOnShutdown(save_on_shutdown_); + // // Configure the GNSS, only if the configuration is different // @@ -1757,7 +1760,7 @@ void HpgRefProduct::subscribe() { &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); } -void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) { +void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { if (enabled["nav_svin"]) { navsvin_pub_.publish(m); } From 13d201112eea702f113e69d4e89d5ac1d1d4a82f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 15:35:06 +0000 Subject: [PATCH 047/152] Add namespaces to component_interface and fts_product.hpp Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/component_interface.hpp | 4 ++++ ublox_gps/include/ublox_gps/fts_product.hpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/ublox_gps/include/ublox_gps/component_interface.hpp b/ublox_gps/include/ublox_gps/component_interface.hpp index 3e973ff5..2d51ad6c 100644 --- a/ublox_gps/include/ublox_gps/component_interface.hpp +++ b/ublox_gps/include/ublox_gps/component_interface.hpp @@ -35,6 +35,8 @@ // to configure the u-blox and subscribe to u-blox messages. // +namespace ublox_node { + /** * @brief This interface is used to add functionality to the main node. * @@ -69,4 +71,6 @@ class ComponentInterface { virtual void subscribe() = 0; }; +} + #endif diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp index 6f378a50..a5f64e89 100644 --- a/ublox_gps/include/ublox_gps/fts_product.hpp +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -34,6 +34,8 @@ #include +namespace ublox_node { + /** * @brief Implements functions for FTS products. Currently unimplemented. * @todo Unimplemented. @@ -67,4 +69,6 @@ class FtsProduct final : public virtual ComponentInterface { void subscribe() override {} }; +} + #endif From 51105418a5f3d66a4203c70548c3eac5ace2d196 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 15:47:09 +0000 Subject: [PATCH 048/152] Make gps a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- .../include/ublox_gps/component_interface.hpp | 8 +- ublox_gps/include/ublox_gps/fts_product.hpp | 12 +- ublox_gps/include/ublox_gps/node.hpp | 45 ++++--- ublox_gps/src/node.cpp | 118 +++++++++--------- 4 files changed, 101 insertions(+), 82 deletions(-) diff --git a/ublox_gps/include/ublox_gps/component_interface.hpp b/ublox_gps/include/ublox_gps/component_interface.hpp index 2d51ad6c..72d8ab92 100644 --- a/ublox_gps/include/ublox_gps/component_interface.hpp +++ b/ublox_gps/include/ublox_gps/component_interface.hpp @@ -30,6 +30,10 @@ #ifndef UBLOX_GPS_COMPONENT_INTERFACE_HPP #define UBLOX_GPS_COMPONENT_INTERFACE_HPP +#include + +#include + // This file declares the ComponentInterface which acts as a high level // interface for u-blox firmware, product categories, etc. It contains methods // to configure the u-blox and subscribe to u-blox messages. @@ -56,7 +60,7 @@ class ComponentInterface { * @brief Configure the U-Blox settings. * @return true if configured correctly, false otherwise */ - virtual bool configureUblox() = 0; + virtual bool configureUblox(std::shared_ptr gps) = 0; /** * @brief Initialize the diagnostics. @@ -68,7 +72,7 @@ class ComponentInterface { /** * @brief Subscribe to u-blox messages and publish to ROS topics. */ - virtual void subscribe() = 0; + virtual void subscribe(std::shared_ptr gps) = 0; }; } diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp index a5f64e89..d279b748 100644 --- a/ublox_gps/include/ublox_gps/fts_product.hpp +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -30,9 +30,12 @@ #ifndef UBLOX_GPS_FTS_PRODUCT_HPP #define UBLOX_GPS_FTS_PRODUCT_HPP +#include + #include #include +#include namespace ublox_node { @@ -54,7 +57,10 @@ class FtsProduct final : public virtual ComponentInterface { * @brief Configure FTS settings. * @todo Currently unimplemented. */ - bool configureUblox() override { return false; } + bool configureUblox(std::shared_ptr gps) override { + (void)gps; + return false; + } /** * @brief Adds diagnostic updaters for FTS status. @@ -66,7 +72,9 @@ class FtsProduct final : public virtual ComponentInterface { * @brief Subscribe to FTS messages. * @todo Currently unimplemented. */ - void subscribe() override {} + void subscribe(std::shared_ptr gps) override { + (void)gps; + } }; } diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 1fa2b6cf..acb2f31e 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -86,8 +86,6 @@ namespace ublox_node { //! Node Handle for GPS node std::shared_ptr nh; -//! Handles communication with the U-Blox Device -std::shared_ptr gps; //! Whether or not to publish the given ublox message /*! * key is the message name (all lowercase) without firmware version numbers @@ -518,6 +516,9 @@ class UbloxNode final { //! Which GNSS are supported by the device std::shared_ptr gnss_; + + //! Handles communication with the U-Blox Device + std::shared_ptr gps_; }; /** @@ -564,12 +565,12 @@ class UbloxFirmware6 final : public UbloxFirmware { * @brief Prints a warning, GNSS configuration not available in this version. * @return true if configured correctly, false otherwise */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; protected: /** @@ -844,14 +845,14 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { /** * @brief Configure GNSS individually. Only configures GLONASS. */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Subscribe to messages which are not generic to all firmware. * * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: //! Used to configure NMEA (if set_nmea_) @@ -892,7 +893,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * Configure the NMEA if desired by the user. It also may clear the * flash memory based on the ROS parameters. */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Subscribe to u-blox messages which are not generic to all firmware @@ -901,7 +902,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based * on user settings. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: // Set from ROS parameters @@ -953,7 +954,10 @@ class RawDataProduct final : public virtual ComponentInterface { * @brief Does nothing since there are no Raw Data product specific settings. * @return always returns true */ - bool configureUblox() override { return true; } + bool configureUblox(std::shared_ptr gps) override { + (void)gps; + return true; + } /** * @brief Adds frequency diagnostics for RTCM topics. @@ -965,7 +969,7 @@ class RawDataProduct final : public virtual ComponentInterface { * * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: //! Topic diagnostic updaters @@ -1001,7 +1005,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { * @details Configure the use_adr setting. * @return true if configured correctly, false otherwise */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Initialize the ROS diagnostics for the ADR/UDR device. @@ -1018,7 +1022,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { * @details Subscribe to NavATT, ESF and HNR messages based on user * parameters. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: //! Whether or not to enable dead reckoning @@ -1076,7 +1080,7 @@ class HpgRefProduct: public virtual ComponentInterface { * the RTCM messages. * @return true if configured correctly, false otherwise */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Add diagnostic updaters for the TMODE3 status. @@ -1088,7 +1092,7 @@ class HpgRefProduct: public virtual ComponentInterface { * * @details Subscribe to NavSVIN messages based on user parameters. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; /** * @brief Update the last received NavSVIN message and call diagnostic updater @@ -1114,7 +1118,7 @@ class HpgRefProduct: public virtual ComponentInterface { * * @details Configure the RTCM messages and measurement and navigation rate. */ - bool setTimeMode(); + bool setTimeMode(std::shared_ptr gps); //! The last received Nav SVIN message ublox_msgs::NavSVIN last_nav_svin_; @@ -1168,6 +1172,7 @@ class HpgRefProduct: public virtual ComponentInterface { std::shared_ptr updater_; std::vector rtcms_; + std::shared_ptr gps_; }; /** @@ -1200,7 +1205,7 @@ class HpgRovProduct final : public virtual ComponentInterface { * @details Configure the DGNSS mode. * @return true if configured correctly, false otherwise */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Add diagnostic updaters for rover GNSS status, including @@ -1211,7 +1216,7 @@ class HpgRovProduct final : public virtual ComponentInterface { /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: /** @@ -1252,7 +1257,7 @@ class HpPosRecProduct final : public virtual HpgRefProduct { /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: @@ -1292,7 +1297,7 @@ class TimProduct final : public virtual ComponentInterface { * @brief Configure Time Sync settings. * @todo Currently unimplemented. */ - bool configureUblox() override; + bool configureUblox(std::shared_ptr gps) override; /** * @brief Adds diagnostic updaters for Time Sync status. @@ -1305,7 +1310,7 @@ class TimProduct final : public virtual ComponentInterface { * * @details Subscribes to RxmRAWX & RxmSFRBX messages. */ - void subscribe() override; + void subscribe(std::shared_ptr gps) override; private: /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f7284eb1..638c5c2c 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -118,7 +118,7 @@ std::vector stringSplit(const std::string &str, UbloxNode::UbloxNode() { int debug; nh->param("debug", debug, 1); - gps = std::make_shared(debug); + gps_ = std::make_shared(debug); gnss_ = std::make_shared(); @@ -293,13 +293,13 @@ void UbloxNode::getRosParams() { void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); if (enabled["aid_alm"]) { - gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } if (enabled["aid_eph"]) { - gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } if (enabled["aid_hui"]) { - gps->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); + gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } payload[0]++; @@ -333,26 +333,26 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, nav_status_pub_), 1); } nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, nav_posecef_pub_), 1); } nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, nav_clock_pub_), 1); } // INF messages nh->param("inf/debug", enabled["inf_debug"], false); if (enabled["inf_debug"]) { - gps->subscribeId( + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); @@ -360,7 +360,7 @@ void UbloxNode::subscribe() { nh->param("inf/error", enabled["inf_error"], enabled["inf"]); if (enabled["inf_error"]) { - gps->subscribeId( + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); @@ -368,7 +368,7 @@ void UbloxNode::subscribe() { nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); if (enabled["inf_notice"]) { - gps->subscribeId( + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); @@ -376,7 +376,7 @@ void UbloxNode::subscribe() { nh->param("inf/test", enabled["inf_test"], enabled["inf"]); if (enabled["inf_test"]) { - gps->subscribeId( + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); @@ -384,7 +384,7 @@ void UbloxNode::subscribe() { nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); if (enabled["inf_warning"]) { - gps->subscribeId( + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); @@ -393,24 +393,24 @@ void UbloxNode::subscribe() { // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, aid_alm_pub_), 1); } nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, aid_eph_pub_), 1); } nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) { - gps->subscribe(std::bind( + gps_->subscribe(std::bind( publish, std::placeholders::_1, aid_hui_pub_), 1); } for (size_t i = 0; i < components_.size(); i++) { - components_[i]->subscribe(); + components_[i]->subscribe(gps_); } } @@ -426,7 +426,7 @@ void UbloxNode::initializeRosDiagnostics() { void UbloxNode::processMonVer() { ublox_msgs::MonVER monVer; - if (!gps->poll(monVer)) { + if (!gps_->poll(monVer)) { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); } @@ -497,19 +497,19 @@ void UbloxNode::processMonVer() { bool UbloxNode::configureUblox() { try { - if (!gps->isInitialized()) + if (!gps_->isInitialized()) throw std::runtime_error("Failed to initialize."); if (load_.load_mask != 0) { ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.load_mask); - if (!gps->configure(load_)) + if (!gps_->configure(load_)) throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); if (load_.load_mask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); - gps->reset(wait); - if (!gps->isConfigured()) + gps_->reset(wait); + if (!gps_->isConfigured()) throw std::runtime_error(std::string("Failed to reset serial I/O") + "after loading I/O configurations from device memory."); } @@ -517,9 +517,9 @@ bool UbloxNode::configureUblox() { if (config_on_startup_flag_) { if (set_usb_) { - gps->configUsb(usb_tx_, usb_in_, usb_out_); + gps_->configUsb(usb_tx_, usb_in_, usb_out_); } - if (!gps->configRate(meas_rate_, nav_rate_)) { + if (!gps_->configRate(meas_rate_, nav_rate_)) { std::stringstream ss; ss << "Failed to set measurement rate to " << meas_rate_ << "ms and navigation rate to " << nav_rate_; @@ -527,34 +527,34 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { - if (!gps->configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { + if (!gps_->configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_sbas_) ? "enable" : "disable") + " SBAS."); } } - if (!gps->setPpp(enable_ppp_)) { + if (!gps_->setPpp(enable_ppp_)) { throw std::runtime_error(std::string("Failed to ") + ((enable_ppp_) ? "enable" : "disable") + " PPP."); } - if (!gps->setDynamicModel(dmodel_)) { + if (!gps_->setDynamicModel(dmodel_)) { throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); } - if (!gps->setFixMode(fmode_)) { + if (!gps_->setFixMode(fmode_)) { throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + "."); } - if (!gps->setDeadReckonLimit(dr_limit_)) { + if (!gps_->setDeadReckonLimit(dr_limit_)) { std::stringstream ss; ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (set_dat_ && !gps->configure(cfg_dat_)) { + if (set_dat_ && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component for (int i = 0; i < components_.size(); i++) { - if (!components_[i]->configureUblox()) { + if (!components_[i]->configureUblox(gps_)) { return false; } } @@ -562,7 +562,7 @@ bool UbloxNode::configureUblox() { if (save_.save_mask != 0) { ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); - if (!gps->configure(save_)) { + if (!gps_->configure(save_)) { ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); } } @@ -602,13 +602,13 @@ void UbloxNode::configureInf() { } ROS_DEBUG("Configuring INF messages"); - if (!gps->configure(msg)) { + if (!gps_->configure(msg)) { ROS_WARN("Failed to configure INF messages"); } } void UbloxNode::initializeIo() { - gps->setConfigOnStartup(config_on_startup_flag_); + gps_->setConfigOnStartup(config_on_startup_flag_); std::smatch match; if (std::regex_match(device_, match, @@ -619,17 +619,17 @@ void UbloxNode::initializeIo() { std::string port(match[3]); ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); - gps->initializeTcp(host, port); + gps_->initializeTcp(host, port); } else { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } } else { - gps->initializeSerial(device_, baudrate_, uart_in_, uart_out_); + gps_->initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging if (rawDataStreamPa_.isEnabled()) { - gps->setRawDataCallback( + gps_->setRawDataCallback( std::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); rawDataStreamPa_.initialize(); } @@ -671,8 +671,8 @@ void UbloxNode::initialize() { } void UbloxNode::shutdown() { - if (gps->isInitialized()) { - gps->close(); + if (gps_->isInitialized()) { + gps_->close(); ROS_INFO("Closed connection to %s.", device_.c_str()); } } @@ -763,7 +763,7 @@ void UbloxFirmware6::getRosParams() { } } -bool UbloxFirmware6::configureUblox() { +bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); if (set_nmea_ && !gps->configure(cfg_nmea_)) { @@ -773,7 +773,7 @@ bool UbloxFirmware6::configureUblox() { return true; } -void UbloxFirmware6::subscribe() { +void UbloxFirmware6::subscribe(std::shared_ptr gps) { // Whether or not to publish Nav POS LLH (always subscribes) nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]); nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]); @@ -1029,7 +1029,7 @@ void UbloxFirmware7::getRosParams() { } } -bool UbloxFirmware7::configureUblox() { +bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { /** Configure the GNSS **/ ublox_msgs::CfgGNSS cfgGNSSRead; if (gps->poll(cfgGNSSRead)) { @@ -1098,7 +1098,7 @@ bool UbloxFirmware7::configureUblox() { return true; } -void UbloxFirmware7::subscribe() { +void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Whether to publish Nav PVT messages to a ROS topic nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT (always does so since fix information is published @@ -1250,7 +1250,7 @@ void UbloxFirmware8::getRosParams() { } -bool UbloxFirmware8::configureUblox() { +bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { if (clear_bbr_) { // clear flash memory if (!gps->clearBbr()) { @@ -1350,7 +1350,7 @@ bool UbloxFirmware8::configureUblox() { return true; } -void UbloxFirmware8::subscribe() { +void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Whether to publish Nav PVT messages nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); // Subscribe to Nav PVT @@ -1395,7 +1395,7 @@ RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::share rxm_alm_pub_ = nh->advertise("rxmalm", 1); } -void RawDataProduct::subscribe() { +void RawDataProduct::subscribe(std::shared_ptr gps) { // Defaults to true instead of to all nh->param("publish/rxm/all", enabled["rxm"], true); @@ -1474,7 +1474,7 @@ void AdrUdrProduct::getRosParams() { } } -bool AdrUdrProduct::configureUblox() { +bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { if (!gps->setUseAdr(use_adr_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); @@ -1482,7 +1482,7 @@ bool AdrUdrProduct::configureUblox() { return true; } -void AdrUdrProduct::subscribe() { +void AdrUdrProduct::subscribe(std::shared_ptr gps) { nh->param("publish/esf/all", enabled["esf"], true); // Subscribe to NAV ATT messages @@ -1680,7 +1680,7 @@ void HpgRefProduct::getRosParams() { } } -bool HpgRefProduct::configureUblox() { +bool HpgRefProduct::configureUblox(std::shared_ptr gps) { // Configure TMODE3 if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { if (!gps->disableTmode3()) { @@ -1710,7 +1710,7 @@ bool HpgRefProduct::configureUblox() { } // Don't reset survey-in if it already has a valid value if (nav_svin.valid) { - setTimeMode(); + setTimeMode(gps); return true; } ublox_msgs::NavPVT nav_pvt; @@ -1721,7 +1721,7 @@ bool HpgRefProduct::configureUblox() { // Don't reset survey in if in time mode with a good fix if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { - setTimeMode(); + setTimeMode(gps); return true; } } @@ -1752,10 +1752,12 @@ bool HpgRefProduct::configureUblox() { return true; } -void HpgRefProduct::subscribe() { +void HpgRefProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Survey-In messages nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); // Subscribe to Nav Survey-In + // Save off the gps pointer so we can use it in the callback later. + gps_ = gps; gps->subscribe(std::bind( &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); } @@ -1768,13 +1770,13 @@ void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { last_nav_svin_ = m; if (!m.active && m.valid && mode_ == SURVEY_IN) { - setTimeMode(); + setTimeMode(gps_); } updater_->update(); } -bool HpgRefProduct::setTimeMode() { +bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { ROS_INFO("Setting mode (internal state) to Time Mode"); mode_ = TIME; @@ -1855,7 +1857,7 @@ void HpgRovProduct::getRosParams() { ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } -bool HpgRovProduct::configureUblox() { +bool HpgRovProduct::configureUblox(std::shared_ptr gps) { // Configure the DGNSS if (!gps->setDgnss(dgnss_mode_)) { throw std::runtime_error(std::string("Failed to Configure DGNSS")); @@ -1863,7 +1865,7 @@ bool HpgRovProduct::configureUblox() { return true; } -void HpgRovProduct::subscribe() { +void HpgRovProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) @@ -1936,7 +1938,7 @@ HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool con nh->advertise("navheading", 1); } -void HpPosRecProduct::subscribe() { +void HpPosRecProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) @@ -1994,7 +1996,7 @@ TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr gps) { uint8_t r = 1; // Configure the reciever if (!gps->setUTCtime()) { @@ -2008,7 +2010,7 @@ bool TimProduct::configureUblox() { return true; } -void TimProduct::subscribe() { +void TimProduct::subscribe(std::shared_ptr gps) { ROS_INFO("TIM is Enabled: %u", enabled["tim"]); ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]); // Subscribe to TIM-TM2 messages (Time mark messages) From 98d635bf00d019c6f4a33c44b742cc982ee50c49 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 15:49:26 +0000 Subject: [PATCH 049/152] Move fixFromString and modelFromString into node.cpp Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 28 ---------------------------- ublox_gps/src/node.cpp | 27 ++++++++++++++++++++++++--- 2 files changed, 24 insertions(+), 31 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index acb2f31e..a949e719 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -96,34 +96,6 @@ std::map enabled; //! based on the enabled GNSS int fix_status_service; -/** - * @brief Determine dynamic model from human-readable string. - * @param model One of the following (case-insensitive): - * - portable - * - stationary - * - pedestrian - * - automotive - * - sea - * - airborne1 - * - airborne2 - * - airborne4 - * - wristwatch - * @return DynamicModel - * @throws std::runtime_error on invalid argument. - */ -uint8_t modelFromString(const std::string& model); - -/** - * @brief Determine fix mode from human-readable string. - * @param mode One of the following (case-insensitive): - * - 2d - * - 3d - * - auto - * @return FixMode - * @throws std::runtime_error on invalid argument. - */ -uint8_t fixModeFromString(const std::string& mode); - /** * @brief Check that the parameter is above the minimum. * @param val the value to check diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 638c5c2c..144e75ca 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -46,9 +46,21 @@ namespace ublox_node { -// -// ublox_node namespace -// +/** + * @brief Determine dynamic model from human-readable string. + * @param model One of the following (case-insensitive): + * - portable + * - stationary + * - pedestrian + * - automotive + * - sea + * - airborne1 + * - airborne2 + * - airborne4 + * - wristwatch + * @return DynamicModel + * @throws std::runtime_error on invalid argument. + */ uint8_t modelFromString(const std::string& model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); @@ -76,6 +88,15 @@ uint8_t modelFromString(const std::string& model) { " is not a valid dynamic model."); } +/** + * @brief Determine fix mode from human-readable string. + * @param mode One of the following (case-insensitive): + * - 2d + * - 3d + * - auto + * @return FixMode + * @throws std::runtime_error on invalid argument. + */ uint8_t fixModeFromString(const std::string& mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); From c6cee600fd5c8cae2bdee8256aeb9f579e0aa667 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 15:57:19 +0000 Subject: [PATCH 050/152] Make fix_status_service a member variable of UbloxFirmware. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 8 ++++---- ublox_gps/src/node.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index a949e719..19b25777 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -92,9 +92,6 @@ std::shared_ptr nh; * (e.g. NavPVT instead of NavPVT7). Value indicates whether or not to enable * the message. */ std::map enabled; -//! The fix status service type, set in the Firmware Component -//! based on the enabled GNSS -int fix_status_service; /** * @brief Check that the parameter is above the minimum. @@ -519,6 +516,9 @@ class UbloxFirmware : public virtual ComponentInterface { std::shared_ptr updater_; std::shared_ptr gnss_; + //! The fix status service type, set in the Firmware Component + //! based on the enabled GNSS + int fix_status_service_{0}; }; /** @@ -683,7 +683,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { fix.status.status = fix.status.STATUS_NO_FIX; } // Set the service based on GNSS configuration - fix.status.service = fix_status_service; + fix.status.service = fix_status_service_; // Set the position covariance const double var_h = pow(m.h_acc / 1000.0, 2); // to [m^2] diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 144e75ca..9181e336 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -740,7 +740,7 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrparam("nmea/set", set_nmea_, false); if (set_nmea_) { @@ -987,7 +987,7 @@ void UbloxFirmware7::getRosParams() { } // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; // @@ -1192,7 +1192,7 @@ void UbloxFirmware8::getRosParams() { } // Fix Service type, used when publishing fix status messages - fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS + fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; From 730c7f73d80869164564da705eb5f26b26e0b38f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 16:57:23 +0000 Subject: [PATCH 051/152] Replace templated publish call with lambdas. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 13 ---- ublox_gps/src/node.cpp | 102 +++++++++++++-------------- 2 files changed, 50 insertions(+), 65 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 19b25777..10bd6a6c 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -262,19 +262,6 @@ bool getRosInt(const std::string& key, std::vector &i) { return true; } -/** - * @brief Publish a ROS message of type MessageT. - * - * @details This function should be used to publish all messages which are - * simply read from u-blox and published. - * @param m the message to publish - * @param topic the topic to publish the message on - */ -template -void publish(const MessageT& m, ros::Publisher & publisher) { - publisher.publish(m); -} - /** * @brief This class represents u-blox ROS node for *all* firmware and product * versions. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 9181e336..f27f92fa 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -354,20 +354,20 @@ void UbloxNode::subscribe() { // Nav Messages nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); if (enabled["nav_status"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, nav_status_pub_), 1); + gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, + 1); } nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); if (enabled["nav_posecef"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, nav_posecef_pub_), 1); + gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, + 1); } nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); if (enabled["nav_clock"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, nav_clock_pub_), 1); + gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, + 1); } // INF messages @@ -414,20 +414,20 @@ void UbloxNode::subscribe() { // AID messages nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); if (enabled["aid_alm"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, aid_alm_pub_), 1); + gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, + 1); } nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); if (enabled["aid_eph"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, aid_eph_pub_), 1); + gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, + 1); } nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); if (enabled["aid_hui"]) { - gps_->subscribe(std::bind( - publish, std::placeholders::_1, aid_hui_pub_), 1); + gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, + 1); } for (size_t i = 0; i < components_.size(); i++) { @@ -814,16 +814,15 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_svinfo_pub_), - kNavSvInfoSubscribeRate); + gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + kNavSvInfoSubscribeRate); } // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), 1); + gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, + 1); } } @@ -1131,16 +1130,15 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Subscribe to Nav SVINFO nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); if (enabled["nav_svinfo"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_svinfo_pub_), - kNavSvInfoSubscribeRate); + gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + kNavSvInfoSubscribeRate); } // Subscribe to Mon HW nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), 1); + gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + 1); } } @@ -1381,22 +1379,22 @@ void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Subscribe to Nav SAT messages nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); if (enabled["nav_sat"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_sat_pub_), kNavSvInfoSubscribeRate); + gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, + kNavSvInfoSubscribeRate); } // Subscribe to Mon HW nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); if (enabled["mon_hw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, mon_hw_pub_), 1); + gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + 1); } // Subscribe to RTCM messages nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); if (enabled["rxm_rtcm"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_rtcm_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, + 1); } } @@ -1423,29 +1421,29 @@ void RawDataProduct::subscribe(std::shared_ptr gps) { // Subscribe to RXM Raw nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_raw_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, + 1); } // Subscribe to RXM SFRB nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_sfrb_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, + 1); } // Subscribe to RXM EPH nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); if (enabled["rxm_eph"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_eph_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, + 1); } // Subscribe to RXM ALM nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); if (enabled["rxm_alm"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_alm_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, + 1); } } @@ -1509,22 +1507,22 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to NAV ATT messages nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); if (enabled["nav_att"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, nav_att_pub_), 1); + gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, + 1); } // Subscribe to ESF INS messages nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); if (enabled["esf_ins"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_ins_pub_), 1); + gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, + 1); } // Subscribe to ESF Meas messages nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); if (enabled["esf_meas"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_meas_pub_), 1); + gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, + 1); // also publish sensor_msgs::Imu gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); @@ -1533,22 +1531,22 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to ESF Raw messages nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); if (enabled["esf_raw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_raw_pub_), 1); + gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, + 1); } // Subscribe to ESF Status messages nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); if (enabled["esf_status"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, esf_status_pub_), 1); + gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, + 1); } // Subscribe to HNR PVT messages nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); if (enabled["hnr_pvt"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, hnr_pvt_pub_), 1); + gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, + 1); } } @@ -2045,15 +2043,15 @@ void TimProduct::subscribe(std::shared_ptr gps) { // Subscribe to SFRBX messages nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); if (enabled["rxm_sfrb"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_sfrb_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, + 1); } // Subscribe to RawX messages nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); if (enabled["rxm_raw"]) { - gps->subscribe(std::bind( - publish, std::placeholders::_1, rxm_raw_pub_), 1); + gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, + 1); } } From b2bba78f58b08b7b8891924a793a1a77ea284ec4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 18:42:10 +0000 Subject: [PATCH 052/152] Add in ROS2-like declare and get parameters. This ensures that a) the parameter is defined on the parameter server, and b) that we can fetch it as a boolean inline. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 27 +++++++++++++++++++++++++-- ublox_gps/src/node.cpp | 12 +++++------- 2 files changed, 30 insertions(+), 9 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 10bd6a6c..fb0ca65b 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -262,6 +262,31 @@ bool getRosInt(const std::string& key, std::vector &i) { return true; } +bool declareRosBoolean(const std::string &name, bool default_value) +{ + bool ret; + + if (!nh->hasParam(name)) { + nh->setParam(name, default_value); + } + // implicit else: If the ROS node already has the parameter, just leave it + + if (!nh->getParam(name, ret)) { + throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); + } +} + +bool getRosBoolean(const std::string &name) +{ + bool ret; + if (!nh->getParam(name, ret)) { + // Note that if this is used after declareRosBoolean, this should never happen. + throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); + } + + return ret; +} + /** * @brief This class represents u-blox ROS node for *all* firmware and product * versions. @@ -425,8 +450,6 @@ class UbloxNode final { ublox_msgs::CfgDAT cfg_dat_; //! Whether or not to enable SBAS bool enable_sbas_; - //! Whether or not to enable PPP (advanced setting) - bool enable_ppp_; //! SBAS Usage parameter (see CfgSBAS message) uint8_t sbas_usage_; //! Max SBAS parameter (see CfgSBAS message) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f27f92fa..9284a94a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -241,7 +241,7 @@ void UbloxNode::getRosParams() { getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting - nh->param("enable_ppp", enable_ppp_, false); + declareRosBoolean("enable_ppp", false); // SBAS params, only for some devices nh->param("sbas", enable_sbas_, false); getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels @@ -250,7 +250,7 @@ void UbloxNode::getRosParams() { nh->param("fix_mode", fix_mode_, std::string("auto")); getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit - if (enable_ppp_) { + if (getRosBoolean("enable_ppp")) { ROS_WARN("Warning: PPP is enabled - this is an expert setting."); } @@ -436,9 +436,7 @@ void UbloxNode::subscribe() { } void UbloxNode::initializeRosDiagnostics() { - if (!nh->hasParam("diagnostic_period")) { - nh->setParam("diagnostic_period", kDiagnosticPeriod); - } + declareRosBoolean("diagnostic_period", kDiagnosticPeriod); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); @@ -554,9 +552,9 @@ bool UbloxNode::configureUblox() { " SBAS."); } } - if (!gps_->setPpp(enable_ppp_)) { + if (!gps_->setPpp(getRosBoolean("enable_ppp"))) { throw std::runtime_error(std::string("Failed to ") + - ((enable_ppp_) ? "enable" : "disable") + (getRosBoolean("enable_ppp") ? "enable" : "disable") + " PPP."); } if (!gps_->setDynamicModel(dmodel_)) { From 73b024134fe6d36689fb78cdc5c5ffda7bfa4149 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 18:55:09 +0000 Subject: [PATCH 053/152] Make sure to declare the sbas parameter. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 4 ---- ublox_gps/src/node.cpp | 26 +++++++++++++------------- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index fb0ca65b..6b0663a8 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -448,8 +448,6 @@ class UbloxNode final { bool set_dat_; //! User-defined Datum ublox_msgs::CfgDAT cfg_dat_; - //! Whether or not to enable SBAS - bool enable_sbas_; //! SBAS Usage parameter (see CfgSBAS message) uint8_t sbas_usage_; //! Max SBAS parameter (see CfgSBAS message) @@ -793,8 +791,6 @@ class UbloxFirmware7Plus : public UbloxFirmware { bool enable_glonass_; //! Whether or not to enable QZSS bool enable_qzss_; - //! Whether or not to enable SBAS - bool enable_sbas_; //! The QZSS Signal configuration, see CfgGNSS message uint32_t qzss_sig_cfg_; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 9284a94a..15b691ef 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -243,7 +243,7 @@ void UbloxNode::getRosParams() { // PPP: Advanced Setting declareRosBoolean("enable_ppp", false); // SBAS params, only for some devices - nh->param("sbas", enable_sbas_, false); + declareRosBoolean("gnss/sbas", false); getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels getRosUint("sbas/usage", sbas_usage_, 0); nh->param("dynamic_model", dynamic_model_, std::string("portable")); @@ -516,13 +516,15 @@ void UbloxNode::processMonVer() { bool UbloxNode::configureUblox() { try { - if (!gps_->isInitialized()) + if (!gps_->isInitialized()) { throw std::runtime_error("Failed to initialize."); + } if (load_.load_mask != 0) { ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.load_mask); - if (!gps_->configure(load_)) + if (!gps_->configure(load_)) { throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); + } if (load_.load_mask & load_.MASK_IO_PORT) { ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", "communications."); @@ -546,9 +548,9 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { - if (!gps_->configSbas(enable_sbas_, sbas_usage_, max_sbas_)) { + if (!gps_->configSbas(getRosBoolean("gnss/sbas"), sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + + (getRosBoolean("gnss/sbas") ? "enable" : "disable") + " SBAS."); } } @@ -957,7 +959,6 @@ void UbloxFirmware7::getRosParams() { nh->param("gnss/qzss", enable_qzss_, false); getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); - nh->param("gnss/sbas", enable_sbas_, false); if (enable_gps_ && !gnss_->isSupported("GPS")) { ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); @@ -969,7 +970,7 @@ void UbloxFirmware7::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (enable_sbas_ && !gnss_->isSupported("SBAS")) { + if (getRosBoolean("gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1100,11 +1101,11 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { block.gnss_id = block.GNSS_ID_SBAS; block.res_trk_ch = block.RES_TRK_CH_SBAS; block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0; + block.flags = getRosBoolean("gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + - ((enable_sbas_) ? "enable" : "disable") + + (getRosBoolean("gnss/sbas") ? "enable" : "disable") + " SBAS."); } } @@ -1155,7 +1156,6 @@ void UbloxFirmware8::getRosParams() { nh->param("gnss/imes", enable_imes_, false); nh->param("gnss/glonass", enable_glonass_, false); nh->param("gnss/qzss", enable_qzss_, false); - nh->param("gnss/sbas", enable_sbas_, false); // QZSS Signal Configuration getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); @@ -1183,7 +1183,7 @@ void UbloxFirmware8::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (enable_sbas_ && !gnss_->isSupported("SBAS")) { + if (getRosBoolean("gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1301,10 +1301,10 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_SBAS - && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) { + && getRosBoolean("gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_; + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean("gnss/sbas"); ROS_DEBUG("SBAS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { From e852f3693ab0b378d8ad0acab01981c89b16d701 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 19:03:21 +0000 Subject: [PATCH 054/152] Make dat/set a declared parameter. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 2 -- ublox_gps/src/node.cpp | 6 +++--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 6b0663a8..90ab3862 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -444,8 +444,6 @@ class UbloxNode final { uint16_t usb_out_ ; //! The measurement rate in Hz double rate_; - //! If true, set configure the User-Defined Datum - bool set_dat_; //! User-defined Datum ublox_msgs::CfgDAT cfg_dat_; //! SBAS Usage parameter (see CfgSBAS message) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 15b691ef..8896b749 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -270,8 +270,8 @@ void UbloxNode::getRosParams() { dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); - nh->param("dat/set", set_dat_, false); - if (set_dat_) { + declareRosBoolean("dat/set", false); + if (getRosBoolean("dat/set")) { std::vector shift, rot; if (!nh->getParam("dat/majA", cfg_dat_.maj_a) || nh->getParam("dat/flat", cfg_dat_.flat) @@ -570,7 +570,7 @@ bool UbloxNode::configureUblox() { ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (set_dat_ && !gps_->configure(cfg_dat_)) { + if (getRosBoolean("dat/set") && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component From af113d6971fb4dd2c2a431c5d08788af18990c59 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 19:06:15 +0000 Subject: [PATCH 055/152] Declare the config_on_startup flag. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 8 ++------ ublox_gps/src/node.cpp | 20 ++++++++++---------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 90ab3862..f837268f 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -475,9 +475,6 @@ class UbloxNode final { //! The measurement [ms], see CfgRate.msg uint16_t meas_rate_; - //! Flag for enabling configuration on startup - bool config_on_startup_flag_; - //! The ROS frame ID of this device std::string frame_id_; @@ -1035,7 +1032,7 @@ class HpgRefProduct: public virtual ComponentInterface { //! Default measurement period for HPG devices constexpr static uint16_t kDefaultMeasPeriod = 250; - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater, std::vector rtcms); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1144,7 +1141,6 @@ class HpgRefProduct: public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; - bool config_on_startup_flag_; std::shared_ptr updater_; std::vector rtcms_; @@ -1228,7 +1224,7 @@ class HpgRovProduct final : public virtual ComponentInterface { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 8896b749..d2a5b07a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -183,11 +183,11 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, updater_, rtcms_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { components_.push_back(std::make_shared(nav_rate_, updater_)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, config_on_startup_flag_, frame_id_, updater_, rtcms_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_)); } else if (product_category.compare("TIM") == 0) { components_.push_back(std::make_shared(frame_id_, updater_)); } else if (product_category.compare("ADR") == 0 || @@ -305,7 +305,7 @@ void UbloxNode::getRosParams() { meas_rate_ = 1000 / rate_; // activate/deactivate any config - nh->param("config_on_startup", config_on_startup_flag_, true); + declareRosBoolean("config_on_startup", true); // raw data stream logging rawDataStreamPa_.getRosParams(); @@ -536,7 +536,7 @@ bool UbloxNode::configureUblox() { } } - if (config_on_startup_flag_) { + if (getRosBoolean("config_on_startup")) { if (set_usb_) { gps_->configUsb(usb_tx_, usb_in_, usb_out_); } @@ -629,7 +629,7 @@ void UbloxNode::configureInf() { } void UbloxNode::initializeIo() { - gps_->setConfigOnStartup(config_on_startup_flag_); + gps_->setConfigOnStartup(getRosBoolean("config_on_startup")); std::smatch match; if (std::regex_match(device_, match, @@ -1645,15 +1645,15 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, std::shared_ptr updater, std::vector rtcms) - : nav_rate_(nav_rate), meas_rate_(meas_rate), config_on_startup_flag_(config_on_startup_flag), updater_(updater), rtcms_(rtcms) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms) { navsvin_pub_ = nh->advertise("navsvin", 1); } void HpgRefProduct::getRosParams() { - if (config_on_startup_flag_) { + if (getRosBoolean("config_on_startup")) { if (nav_rate_ * meas_rate_ != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); } @@ -1945,8 +1945,8 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, bool config_on_startup_flag, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms) - : HpgRefProduct(nav_rate, meas_rate, config_on_startup_flag, updater, rtcms), frame_id_(frame_id) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms) + : HpgRefProduct(nav_rate, meas_rate, updater, rtcms), frame_id_(frame_id) { nav_relposned_pub_ = nh->advertise("navrelposned", 1); From 1f8b7d7ebedd20f16e2c439101a9ea86f53c1b97 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 19:24:31 +0000 Subject: [PATCH 056/152] Convert a few more parameters over to being declared. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 6 -- ublox_gps/src/node.cpp | 141 +++++++++++++-------------- 2 files changed, 69 insertions(+), 78 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index f837268f..cf889153 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -593,8 +593,6 @@ class UbloxFirmware6 final : public UbloxFirmware { //! Used to configure NMEA (if set_nmea_) filled with ROS parameters ublox_msgs::CfgNMEA6 cfg_nmea_; - //! Whether or not to configure the NMEA settings - bool set_nmea_; ros::Publisher nav_pos_llh_pub_; ros::Publisher fix_pub_; @@ -833,8 +831,6 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { * Filled from ROS parameters */ ublox_msgs::CfgNMEA7 cfg_nmea_; - //! Whether or not to Configure the NMEA settings - bool set_nmea_; ros::Publisher nav_svinfo_pub_; ros::Publisher mon_hw_pub_; @@ -885,8 +881,6 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { bool enable_beidou_; //! Whether or not to enable the IMES GNSS bool enable_imes_; - //! Whether or not to configure the NMEA settings - bool set_nmea_; //! Desired NMEA configuration. ublox_msgs::CfgNMEA cfg_nmea_; //! Whether to clear the flash memory during configuration diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d2a5b07a..1e44f73e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -309,17 +309,19 @@ void UbloxNode::getRosParams() { // raw data stream logging rawDataStreamPa_.getRosParams(); + + declareRosBoolean("nmea/set", false); } void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); - if (enabled["aid_alm"]) { + if (getRosBoolean("publish/aid/alm")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } - if (enabled["aid_eph"]) { + if (getRosBoolean("publish/aid/eph")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } - if (enabled["aid_hui"]) { + if (getRosBoolean("publish/aid/hui")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } @@ -344,67 +346,67 @@ void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { void UbloxNode::subscribe() { ROS_DEBUG("Subscribing to U-Blox messages"); // subscribe messages - nh->param("publish/all", enabled["all"], false); - nh->param("inf/all", enabled["inf"], true); - nh->param("publish/nav/all", enabled["nav"], enabled["all"]); - nh->param("publish/rxm/all", enabled["rxm"], enabled["all"]); - nh->param("publish/aid/all", enabled["aid"], enabled["all"]); - nh->param("publish/mon/all", enabled["mon"], enabled["all"]); + declareRosBoolean("publish/all", false); + declareRosBoolean("inf/all", true); + declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/rxm/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/aid/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/mon/all", getRosBoolean("publish/all")); // Nav Messages - nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]); - if (enabled["nav_status"]) { + declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); + if (getRosBoolean("publish/nav/status")) { gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, 1); } - nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]); - if (enabled["nav_posecef"]) { + declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); + if (getRosBoolean("publish/nav/posecef")) { gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, 1); } - nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]); - if (enabled["nav_clock"]) { + declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); + if (getRosBoolean("publish/nav/clock")) { gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, 1); } // INF messages - nh->param("inf/debug", enabled["inf_debug"], false); - if (enabled["inf_debug"]) { + declareRosBoolean("inf/debug", false); + if (getRosBoolean("inf/debug")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); } - nh->param("inf/error", enabled["inf_error"], enabled["inf"]); - if (enabled["inf_error"]) { + declareRosBoolean("inf/error", getRosBoolean("inf/all")); + if (getRosBoolean("inf/error")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); } - nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]); - if (enabled["inf_notice"]) { + declareRosBoolean("inf/notice", getRosBoolean("inf/all")); + if (getRosBoolean("inf/notice")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); } - nh->param("inf/test", enabled["inf_test"], enabled["inf"]); - if (enabled["inf_test"]) { + declareRosBoolean("inf/test", getRosBoolean("inf/all")); + if (getRosBoolean("inf/test")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); } - nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]); - if (enabled["inf_warning"]) { + declareRosBoolean("inf/warning", getRosBoolean("inf/all")); + if (getRosBoolean("inf/warning")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), @@ -412,20 +414,20 @@ void UbloxNode::subscribe() { } // AID messages - nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]); - if (enabled["aid_alm"]) { + declareRosBoolean("publish/aid/alm", getRosBoolean("publish/aid/all")); + if (getRosBoolean("publish/aid/alm")) { gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, 1); } - nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]); - if (enabled["aid_eph"]) { + declareRosBoolean("publish/aid/eph", getRosBoolean("publish/aid/all")); + if (getRosBoolean("publish/aid/eph")) { gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, 1); } - nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]); - if (enabled["aid_hui"]) { + declareRosBoolean("publish/aid/hui", getRosBoolean("publish/aid/all")); + if (getRosBoolean("publish/aid/hui")) { gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, 1); } @@ -600,11 +602,11 @@ void UbloxNode::configureInf() { ublox_msgs::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) | - (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) | - (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) | - (enabled["inf_test"] ? block.INF_MSG_TEST : 0) | - (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0); + uint8_t mask = (getRosBoolean("inf/error") ? block.INF_MSG_ERROR : 0) | + (getRosBoolean("inf/warning") ? block.INF_MSG_WARNING : 0) | + (getRosBoolean("inf/notice") ? block.INF_MSG_NOTICE : 0) | + (getRosBoolean("inf/test") ? block.INF_MSG_TEST : 0) | + (getRosBoolean("inf/debug") ? block.INF_MSG_DEBUG : 0); for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } @@ -663,7 +665,8 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) { - if (nh->param("raw_data", false)) { + declareRosBoolean("raw_data", false); + if (getRosBoolean("raw_data")) { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_)); } } @@ -742,8 +745,7 @@ void UbloxFirmware6::getRosParams() { // Fix Service type, used when publishing fix status messages fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (getRosBoolean("nmea/set")) { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.version)) { @@ -787,7 +789,7 @@ void UbloxFirmware6::getRosParams() { bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - if (set_nmea_ && !gps->configure(cfg_nmea_)) { + if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -796,9 +798,9 @@ bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { void UbloxFirmware6::subscribe(std::shared_ptr gps) { // Whether or not to publish Nav POS LLH (always subscribes) - nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]); - nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]); - nh->param("publish/nav/velned", enabled["nav_velned"], enabled["nav"]); + nh->param("publish/nav/posllh", enabled["nav_posllh"], getRosBoolean("publish/nav/all")); + nh->param("publish/nav/sol", enabled["nav_sol"], getRosBoolean("publish/nav/all")); + nh->param("publish/nav/velned", enabled["nav_velned"], getRosBoolean("publish/nav/all")); // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH @@ -812,14 +814,14 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); + nh->param("publish/nav/svinfo", enabled["nav_svinfo"], getRosBoolean("publish/nav/all")); if (enabled["nav_svinfo"]) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); + nh->param("publish/mon_hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); if (enabled["mon_hw"]) { gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, 1); @@ -991,8 +993,7 @@ void UbloxFirmware7::getRosParams() { // // NMEA Configuration // - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (getRosBoolean("nmea/set")) { bool compat, consider; if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) { @@ -1110,7 +1111,7 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { } } - if (set_nmea_ && !gps->configure(cfg_nmea_)) { + if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1119,7 +1120,7 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Whether to publish Nav PVT messages to a ROS topic - nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); + nh->param("publish/nav/pvt", enabled["nav_pvt"], getRosBoolean("publish/nav/all")); // Subscribe to Nav PVT (always does so since fix information is published // from this) gps->subscribe(std::bind( @@ -1127,14 +1128,14 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { 1); // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]); + nh->param("publish/nav/svinfo", enabled["nav_svinfo"], getRosBoolean("publish/nav/all")); if (enabled["nav_svinfo"]) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]); + nh->param("publish/mon_hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); if (enabled["mon_hw"]) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); @@ -1196,8 +1197,7 @@ void UbloxFirmware8::getRosParams() { // // NMEA Configuration // - nh->param("nmea/set", set_nmea_, false); - if (set_nmea_) { + if (getRosBoolean("nmea/set")) { bool compat, consider; cfg_nmea_.version = cfg_nmea_.VERSION; // message version @@ -1360,7 +1360,7 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // // NMEA config // - if (set_nmea_ && !gps->configure(cfg_nmea_)) { + if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1369,27 +1369,27 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Whether to publish Nav PVT messages - nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]); + nh->param("publish/nav/pvt", enabled["nav_pvt"], getRosBoolean("publish/nav/all")); // Subscribe to Nav PVT gps->subscribe( std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SAT messages - nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]); + nh->param("publish/nav/sat", enabled["nav_sat"], getRosBoolean("publish/nav/all")); if (enabled["nav_sat"]) { gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]); + nh->param("publish/mon/hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); if (enabled["mon_hw"]) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } // Subscribe to RTCM messages - nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]); + nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_rtcm"]) { gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, 1); @@ -1413,32 +1413,29 @@ RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::share } void RawDataProduct::subscribe(std::shared_ptr gps) { - // Defaults to true instead of to all - nh->param("publish/rxm/all", enabled["rxm"], true); - // Subscribe to RXM Raw - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); + nh->param("publish/rxm/raw", enabled["rxm_raw"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_raw"]) { gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, 1); } // Subscribe to RXM SFRB - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); + nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_sfrb"]) { gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RXM EPH - nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]); + nh->param("publish/rxm/eph", enabled["rxm_eph"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_eph"]) { gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, 1); } // Subscribe to RXM ALM - nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]); + nh->param("publish/rxm/almRaw", enabled["rxm_alm"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_alm"]) { gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, 1); @@ -1503,7 +1500,7 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { nh->param("publish/esf/all", enabled["esf"], true); // Subscribe to NAV ATT messages - nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]); + nh->param("publish/nav/att", enabled["nav_att"], getRosBoolean("publish/nav/all")); if (enabled["nav_att"]) { gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, 1); @@ -1771,7 +1768,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { void HpgRefProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Survey-In messages - nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]); + nh->param("publish/nav/svin", enabled["nav_svin"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Survey-In // Save off the gps pointer so we can use it in the callback later. gps_ = gps; @@ -1884,7 +1881,7 @@ bool HpgRovProduct::configureUblox(std::shared_ptr gps) { void HpgRovProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); + nh->param("publish/nav/relposned", enabled["nav_relposned"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); @@ -1957,13 +1954,13 @@ HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const st void HpPosRecProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]); + nh->param("publish/nav/relposned", enabled["nav_relposned"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); // Whether to publish the Heading info from Nav Relative Position NED - nh->param("publish/nav/heading", enabled["nav_heading"], enabled["nav"]); + nh->param("publish/nav/heading", enabled["nav_heading"], getRosBoolean("publish/nav/all")); } void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { @@ -2039,14 +2036,14 @@ void TimProduct::subscribe(std::shared_ptr gps) { ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]); + nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_sfrb"]) { gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RawX messages - nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]); + nh->param("publish/rxm/raw", enabled["rxm_raw"], getRosBoolean("publish/rxm/all")); if (enabled["rxm_raw"]) { gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, 1); From 5054f0f07057caf7813ad700d3198af7c98c5dbd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 19:51:47 +0000 Subject: [PATCH 057/152] Declare more parameters. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 114 ++++++++++++++++++++--------------------- 1 file changed, 56 insertions(+), 58 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 1e44f73e..662a88bb 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -306,11 +306,51 @@ void UbloxNode::getRosParams() { // activate/deactivate any config declareRosBoolean("config_on_startup", true); + declareRosBoolean("raw_data", false); // raw data stream logging rawDataStreamPa_.getRosParams(); + // NMEA parameters declareRosBoolean("nmea/set", false); + declareRosBoolean("nmea/filter/pos", false); + declareRosBoolean("nmea/filter/msk_pos", false); + declareRosBoolean("nmea/filter/time", false); + declareRosBoolean("nmea/filter/date", false); + declareRosBoolean("nmea/filter/sbas", false); + declareRosBoolean("nmea/filter/track", false); + declareRosBoolean("nmea/compat", false); + declareRosBoolean("nmea/consider", false); + + // Publish parameters + declareRosBoolean("publish/all", false); + + declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/posllh", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/sol", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/svinfo", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/velned", getRosBoolean("publish/nav/all")); + + declareRosBoolean("publish/rxm/all", getRosBoolean("publish/all")); + + declareRosBoolean("publish/aid/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/aid/alm", getRosBoolean("publish/aid/all")); + declareRosBoolean("publish/aid/eph", getRosBoolean("publish/aid/all")); + declareRosBoolean("publish/aid/hui", getRosBoolean("publish/aid/all")); + + declareRosBoolean("publish/mon/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/mon/hw", getRosBoolean("publish/mon/all")); + + // INF parameters + declareRosBoolean("inf/all", true); + declareRosBoolean("inf/debug", false); + declareRosBoolean("inf/error", getRosBoolean("inf/all")); + declareRosBoolean("inf/notice", getRosBoolean("inf/all")); + declareRosBoolean("inf/test", getRosBoolean("inf/all")); + declareRosBoolean("inf/warning", getRosBoolean("inf/all")); } void UbloxNode::pollMessages(const ros::TimerEvent& event) { @@ -346,34 +386,24 @@ void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { void UbloxNode::subscribe() { ROS_DEBUG("Subscribing to U-Blox messages"); // subscribe messages - declareRosBoolean("publish/all", false); - declareRosBoolean("inf/all", true); - declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/rxm/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/aid/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/mon/all", getRosBoolean("publish/all")); // Nav Messages - declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); if (getRosBoolean("publish/nav/status")) { gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, 1); } - declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); if (getRosBoolean("publish/nav/posecef")) { gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, 1); } - declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); if (getRosBoolean("publish/nav/clock")) { gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, 1); } // INF messages - declareRosBoolean("inf/debug", false); if (getRosBoolean("inf/debug")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, @@ -381,7 +411,6 @@ void UbloxNode::subscribe() { ublox_msgs::Message::INF::DEBUG); } - declareRosBoolean("inf/error", getRosBoolean("inf/all")); if (getRosBoolean("inf/error")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, @@ -389,7 +418,6 @@ void UbloxNode::subscribe() { ublox_msgs::Message::INF::ERROR); } - declareRosBoolean("inf/notice", getRosBoolean("inf/all")); if (getRosBoolean("inf/notice")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, @@ -397,7 +425,6 @@ void UbloxNode::subscribe() { ublox_msgs::Message::INF::NOTICE); } - declareRosBoolean("inf/test", getRosBoolean("inf/all")); if (getRosBoolean("inf/test")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, @@ -405,7 +432,6 @@ void UbloxNode::subscribe() { ublox_msgs::Message::INF::TEST); } - declareRosBoolean("inf/warning", getRosBoolean("inf/all")); if (getRosBoolean("inf/warning")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, @@ -414,19 +440,16 @@ void UbloxNode::subscribe() { } // AID messages - declareRosBoolean("publish/aid/alm", getRosBoolean("publish/aid/all")); if (getRosBoolean("publish/aid/alm")) { gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, 1); } - declareRosBoolean("publish/aid/eph", getRosBoolean("publish/aid/all")); if (getRosBoolean("publish/aid/eph")) { gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, 1); } - declareRosBoolean("publish/aid/hui", getRosBoolean("publish/aid/all")); if (getRosBoolean("publish/aid/hui")) { gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, 1); @@ -665,7 +688,6 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) { - declareRosBoolean("raw_data", false); if (getRosBoolean("raw_data")) { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_)); } @@ -756,33 +778,19 @@ void UbloxFirmware6::getRosParams() { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } - if (!nh->getParam("nmea/compat", compat)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - } - if (!nh->getParam("nmea/consider", consider)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - } // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = getRosBoolean("nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean("nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter bool temp; - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/sbas", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_SBAS_FILT : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; } } @@ -797,11 +805,6 @@ bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { } void UbloxFirmware6::subscribe(std::shared_ptr gps) { - // Whether or not to publish Nav POS LLH (always subscribes) - nh->param("publish/nav/posllh", enabled["nav_posllh"], getRosBoolean("publish/nav/all")); - nh->param("publish/nav/sol", enabled["nav_sol"], getRosBoolean("publish/nav/all")); - nh->param("publish/nav/velned", enabled["nav_velned"], getRosBoolean("publish/nav/all")); - // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH gps->subscribe(std::bind( @@ -814,15 +817,13 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], getRosBoolean("publish/nav/all")); - if (enabled["nav_svinfo"]) { + if (getRosBoolean("publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); - if (enabled["mon_hw"]) { + if (getRosBoolean("publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, 1); } @@ -871,7 +872,7 @@ void UbloxFirmware6::fixDiagnostic( } void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if (enabled["nav_posllh"]) { + if (getRosBoolean("publish/nav/posllh")) { nav_pos_llh_pub_.publish(m); } @@ -912,7 +913,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if (enabled["nav_velned"]) { + if (getRosBoolean("publish/nav/velned")) { nav_vel_ned_pub_.publish(m); } @@ -942,7 +943,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { } void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if (enabled["nav_sol"]) { + if (getRosBoolean("publish/nav/sol")) { nav_sol_pub_.publish(m); } last_nav_sol_ = m; @@ -1128,15 +1129,13 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { 1); // Subscribe to Nav SVINFO - nh->param("publish/nav/svinfo", enabled["nav_svinfo"], getRosBoolean("publish/nav/all")); - if (enabled["nav_svinfo"]) { + if (getRosBoolean("publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - nh->param("publish/mon_hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); - if (enabled["mon_hw"]) { + if (getRosBoolean("publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } @@ -1382,8 +1381,7 @@ void UbloxFirmware8::subscribe(std::shared_ptr gps) { } // Subscribe to Mon HW - nh->param("publish/mon/hw", enabled["mon_hw"], getRosBoolean("publish/mon/all")); - if (enabled["mon_hw"]) { + if (getRosBoolean("publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } From 895c15f71ff760a44fe899bd76d773a24db90cdd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 20:01:51 +0000 Subject: [PATCH 058/152] Move some more boolean parameters to declarations. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 75 ++++++++++++++++++++---------------------- 1 file changed, 36 insertions(+), 39 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 662a88bb..5c08f750 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -326,15 +326,22 @@ void UbloxNode::getRosParams() { declareRosBoolean("publish/all", false); declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/nav/att", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/posllh", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/sat", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/sol", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/svinfo", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/velned", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/rxm/all", getRosBoolean("publish/all")); + declareRosBoolean("publish/rxm/almRaw", getRosBoolean("publish/rxm/all")); + declareRosBoolean("publish/rxm/eph", getRosBoolean("publish/rxm/all")); + declareRosBoolean("publish/rxm/rtcm", getRosBoolean("publish/rxm/all")); + declareRosBoolean("publish/rxm/raw", getRosBoolean("publish/rxm/all")); + declareRosBoolean("publish/rxm/sfrb", getRosBoolean("publish/rxm/all")); declareRosBoolean("publish/aid/all", getRosBoolean("publish/all")); declareRosBoolean("publish/aid/alm", getRosBoolean("publish/aid/all")); @@ -351,6 +358,16 @@ void UbloxNode::getRosParams() { declareRosBoolean("inf/notice", getRosBoolean("inf/all")); declareRosBoolean("inf/test", getRosBoolean("inf/all")); declareRosBoolean("inf/warning", getRosBoolean("inf/all")); + + // ESF parameters + declareRosBoolean("publish/esf/all", true); + declareRosBoolean("publish/esf/ins", enabled["esf"]); + declareRosBoolean("publish/esf/meas", enabled["esf"]); + declareRosBoolean("publish/esf/raw", enabled["esf"]); + declareRosBoolean("publish/esf/status", enabled["esf"]); + + // HNR parameters + declareRosBoolean("publish/hnr/pvt", true); } void UbloxNode::pollMessages(const ros::TimerEvent& event) { @@ -1120,8 +1137,6 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { } void UbloxFirmware7::subscribe(std::shared_ptr gps) { - // Whether to publish Nav PVT messages to a ROS topic - nh->param("publish/nav/pvt", enabled["nav_pvt"], getRosBoolean("publish/nav/all")); // Subscribe to Nav PVT (always does so since fix information is published // from this) gps->subscribe(std::bind( @@ -1367,15 +1382,12 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { } void UbloxFirmware8::subscribe(std::shared_ptr gps) { - // Whether to publish Nav PVT messages - nh->param("publish/nav/pvt", enabled["nav_pvt"], getRosBoolean("publish/nav/all")); // Subscribe to Nav PVT gps->subscribe( std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SAT messages - nh->param("publish/nav/sat", enabled["nav_sat"], getRosBoolean("publish/nav/all")); - if (enabled["nav_sat"]) { + if (getRosBoolean("publish/nav/sat")) { gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, kNavSvInfoSubscribeRate); } @@ -1387,8 +1399,7 @@ void UbloxFirmware8::subscribe(std::shared_ptr gps) { } // Subscribe to RTCM messages - nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_rtcm"]) { + if (getRosBoolean("publish/rxm/rtcm")) { gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, 1); } @@ -1412,48 +1423,44 @@ RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::share void RawDataProduct::subscribe(std::shared_ptr gps) { // Subscribe to RXM Raw - nh->param("publish/rxm/raw", enabled["rxm_raw"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_raw"]) { + if (getRosBoolean("publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, 1); } // Subscribe to RXM SFRB - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_sfrb"]) { + if (getRosBoolean("publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RXM EPH - nh->param("publish/rxm/eph", enabled["rxm_eph"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_eph"]) { + if (getRosBoolean("publish/rxm/eph")) { gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, 1); } // Subscribe to RXM ALM - nh->param("publish/rxm/almRaw", enabled["rxm_alm"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_alm"]) { + if (getRosBoolean("publish/rxm/almRaw")) { gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, 1); } } void RawDataProduct::initializeRosDiagnostics() { - if (enabled["rxm_raw"]) { + if (getRosBoolean("publish/rxm/raw")) { freq_diagnostics_.push_back(std::make_shared( "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (enabled["rxm_sfrb"]) { + if (getRosBoolean("publish/rxm/sfrb")) { freq_diagnostics_.push_back(std::make_shared( "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (enabled["rxm_eph"]) { + if (getRosBoolean("publish/rxm/eph")) { freq_diagnostics_.push_back(std::make_shared( "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (enabled["rxm_alm"]) { + if (getRosBoolean("publish/rxm/almRaw")) { freq_diagnostics_.push_back(std::make_shared( "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } @@ -1495,25 +1502,20 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { } void AdrUdrProduct::subscribe(std::shared_ptr gps) { - nh->param("publish/esf/all", enabled["esf"], true); - // Subscribe to NAV ATT messages - nh->param("publish/nav/att", enabled["nav_att"], getRosBoolean("publish/nav/all")); - if (enabled["nav_att"]) { + if (getRosBoolean("publish/nav/att")) { gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, 1); } // Subscribe to ESF INS messages - nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]); - if (enabled["esf_ins"]) { + if (getRosBoolean("publish/esf/ins")) { gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, 1); } // Subscribe to ESF Meas messages - nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]); - if (enabled["esf_meas"]) { + if (getRosBoolean("publish/esf/meas")) { gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, 1); // also publish sensor_msgs::Imu @@ -1522,29 +1524,26 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { } // Subscribe to ESF Raw messages - nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]); - if (enabled["esf_raw"]) { + if (getRosBoolean("publish/esf/raw")) { gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, 1); } // Subscribe to ESF Status messages - nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]); - if (enabled["esf_status"]) { + if (getRosBoolean("publish/esf/status")) { gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, 1); } // Subscribe to HNR PVT messages - nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true); - if (enabled["hnr_pvt"]) { + if (getRosBoolean("publish/hnr/pvt")) { gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, 1); } } void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (enabled["esf_meas"]) { + if (getRosBoolean("publish/esf/meas")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -2034,15 +2033,13 @@ void TimProduct::subscribe(std::shared_ptr gps) { ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages - nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_sfrb"]) { + if (getRosBoolean("publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RawX messages - nh->param("publish/rxm/raw", enabled["rxm_raw"], getRosBoolean("publish/rxm/all")); - if (enabled["rxm_raw"]) { + if (getRosBoolean("publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, 1); } From eed32ef86454a476afa61d5be68e58c1befe4393 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 27 Nov 2019 20:15:48 +0000 Subject: [PATCH 059/152] Finish removing the "enable" map. It was really duplicating information that should have been available via the ROS parameters, so just switch to that everywhere. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 9 +------ ublox_gps/src/node.cpp | 37 +++++++++++----------------- 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index cf889153..6e3cbf7a 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -86,13 +86,6 @@ namespace ublox_node { //! Node Handle for GPS node std::shared_ptr nh; -//! Whether or not to publish the given ublox message -/*! - * key is the message name (all lowercase) without firmware version numbers - * (e.g. NavPVT instead of NavPVT7). Value indicates whether or not to enable - * the message. */ -std::map enabled; - /** * @brief Check that the parameter is above the minimum. * @param val the value to check @@ -639,7 +632,7 @@ class UbloxFirmware7Plus : public UbloxFirmware { * @param m the message to publish */ void callbackNavPvt(const NavPVT& m) { - if (enabled["nav_pvt"]) { + if (getRosBoolean("publish/nav/pvt")) { // NavPVT publisher nav_pvt_pub_.publish(m); } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 5c08f750..85b16b7b 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -328,10 +328,14 @@ void UbloxNode::getRosParams() { declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); declareRosBoolean("publish/nav/att", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/heading", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/posllh", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/pvt", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/relposned", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/sat", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/sol", getRosBoolean("publish/nav/all")); + declareRosBoolean("publish/nav/svin", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/svinfo", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); declareRosBoolean("publish/nav/velned", getRosBoolean("publish/nav/all")); @@ -351,6 +355,8 @@ void UbloxNode::getRosParams() { declareRosBoolean("publish/mon/all", getRosBoolean("publish/all")); declareRosBoolean("publish/mon/hw", getRosBoolean("publish/mon/all")); + declareRosBoolean("publish/tim/tm2", false); + // INF parameters declareRosBoolean("inf/all", true); declareRosBoolean("inf/debug", false); @@ -361,10 +367,10 @@ void UbloxNode::getRosParams() { // ESF parameters declareRosBoolean("publish/esf/all", true); - declareRosBoolean("publish/esf/ins", enabled["esf"]); - declareRosBoolean("publish/esf/meas", enabled["esf"]); - declareRosBoolean("publish/esf/raw", enabled["esf"]); - declareRosBoolean("publish/esf/status", enabled["esf"]); + declareRosBoolean("publish/esf/ins", getRosBoolean("publish/esf/all")); + declareRosBoolean("publish/esf/meas", getRosBoolean("publish/esf/all")); + declareRosBoolean("publish/esf/raw", getRosBoolean("publish/esf/all")); + declareRosBoolean("publish/esf/status", getRosBoolean("publish/esf/all")); // HNR parameters declareRosBoolean("publish/hnr/pvt", true); @@ -1764,8 +1770,6 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { } void HpgRefProduct::subscribe(std::shared_ptr gps) { - // Whether to publish Nav Survey-In messages - nh->param("publish/nav/svin", enabled["nav_svin"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Survey-In // Save off the gps pointer so we can use it in the callback later. gps_ = gps; @@ -1774,7 +1778,7 @@ void HpgRefProduct::subscribe(std::shared_ptr gps) { } void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { - if (enabled["nav_svin"]) { + if (getRosBoolean("publish/nav/svin")) { navsvin_pub_.publish(m); } @@ -1877,8 +1881,6 @@ bool HpgRovProduct::configureUblox(std::shared_ptr gps) { } void HpgRovProduct::subscribe(std::shared_ptr gps) { - // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); @@ -1928,7 +1930,7 @@ void HpgRovProduct::carrierPhaseDiagnostics( } void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (enabled["nav_relposned"]) { + if (getRosBoolean("publish/nav/relposned")) { nav_rel_pos_ned_pub_.publish(m); } @@ -1951,21 +1953,17 @@ HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const st void HpPosRecProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED - nh->param("publish/nav/relposned", enabled["nav_relposned"], getRosBoolean("publish/nav/all")); // Subscribe to Nav Relative Position NED messages (also updates diagnostics) gps->subscribe(std::bind( &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); - - // Whether to publish the Heading info from Nav Relative Position NED - nh->param("publish/nav/heading", enabled["nav_heading"], getRosBoolean("publish/nav/all")); } void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (enabled["nav_relposned"]) { + if (getRosBoolean("publish/nav/relposned")) { nav_relposned_pub_.publish(m); } - if (enabled["nav_heading"]) { + if (getRosBoolean("publish/nav/heading")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -2022,11 +2020,6 @@ bool TimProduct::configureUblox(std::shared_ptr gps) { } void TimProduct::subscribe(std::shared_ptr gps) { - ROS_INFO("TIM is Enabled: %u", enabled["tim"]); - ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]); - // Subscribe to TIM-TM2 messages (Time mark messages) - nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]); - gps->subscribe(std::bind( &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); @@ -2047,7 +2040,7 @@ void TimProduct::subscribe(std::shared_ptr gps) { void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - if (enabled["tim_tm2"]) { + if (getRosBoolean("publish/tim/tm2")) { // create time ref message and put in the data t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); From e004a69ed6dc7f8fed11fc6d8b519e7d7608fc77 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 13:40:29 +0000 Subject: [PATCH 060/152] Declare a lot more parameters. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 116 +++++++++++++++++++---------------------- 1 file changed, 54 insertions(+), 62 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85b16b7b..99064b85 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -244,6 +244,12 @@ void UbloxNode::getRosParams() { declareRosBoolean("enable_ppp", false); // SBAS params, only for some devices declareRosBoolean("gnss/sbas", false); + declareRosBoolean("gnss/gps", true); + declareRosBoolean("gnss/glonass", false); + declareRosBoolean("gnss/qzss", false); + declareRosBoolean("gnss/galileo", false); + declareRosBoolean("gnss/beidou", false); + declareRosBoolean("gnss/imes", false); getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels getRosUint("sbas/usage", sbas_usage_, 0); nh->param("dynamic_model", dynamic_model_, std::string("portable")); @@ -307,20 +313,30 @@ void UbloxNode::getRosParams() { // activate/deactivate any config declareRosBoolean("config_on_startup", true); declareRosBoolean("raw_data", false); + declareRosBoolean("clear_bbr", false); + declareRosBoolean("save_on_shutdown", false); // raw data stream logging rawDataStreamPa_.getRosParams(); // NMEA parameters declareRosBoolean("nmea/set", false); + declareRosBoolean("nmea/compat", false); + declareRosBoolean("nmea/consider", false); + declareRosBoolean("nmea/limit82", false); + declareRosBoolean("nmea/high_prec", false); declareRosBoolean("nmea/filter/pos", false); declareRosBoolean("nmea/filter/msk_pos", false); declareRosBoolean("nmea/filter/time", false); declareRosBoolean("nmea/filter/date", false); declareRosBoolean("nmea/filter/sbas", false); declareRosBoolean("nmea/filter/track", false); - declareRosBoolean("nmea/compat", false); - declareRosBoolean("nmea/consider", false); + declareRosBoolean("nmea/filter/gps_only", false); + declareRosBoolean("nmea/gnssToFilter/gps", false); + declareRosBoolean("nmea/gnssToFilter/sbas", false); + declareRosBoolean("nmea/gnssToFilter/qzss", false); + declareRosBoolean("nmea/gnssToFilter/glonass", false); + declareRosBoolean("nmea/gnssToFilter/beidou", false); // Publish parameters declareRosBoolean("publish/all", false); @@ -980,9 +996,10 @@ void UbloxFirmware7::getRosParams() { // GNSS configuration // // GNSS enable/disable - nh->param("gnss/gps", enable_gps_, true); - nh->param("gnss/glonass", enable_glonass_, false); - nh->param("gnss/qzss", enable_qzss_, false); + enable_gps_ = getRosBoolean("gnss/gps"); + enable_glonass_ = getRosBoolean("gnss/glonass"); + enable_qzss_ = getRosBoolean("gnss/qzss"); + getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); @@ -1045,28 +1062,17 @@ void UbloxFirmware7::getRosParams() { cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - bool temp; - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/gps_only", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); @@ -1167,16 +1173,17 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // void UbloxFirmware8::getRosParams() { // UPD SOS configuration - nh->param("clear_bbr", clear_bbr_, false); - nh->param("save_on_shutdown", save_on_shutdown_, false); + clear_bbr_ = getRosBoolean("clear_bbr"); + save_on_shutdown_ = getRosBoolean("save_on_shutdown"); // GNSS enable/disable - nh->param("gnss/gps", enable_gps_, true); - nh->param("gnss/galileo", enable_galileo_, false); - nh->param("gnss/beidou", enable_beidou_, false); - nh->param("gnss/imes", enable_imes_, false); - nh->param("gnss/glonass", enable_glonass_, false); - nh->param("gnss/qzss", enable_qzss_, false); + enable_gps_ = getRosBoolean("gnss/gps"); + enable_galileo_ = getRosBoolean("gnss/galileo"); + enable_beidou_ = getRosBoolean("gnss/beidou"); + enable_imes_ = getRosBoolean("gnss/imes"); + enable_glonass_ = getRosBoolean("gnss/glonass"); + enable_qzss_ = getRosBoolean("gnss/qzss"); + // QZSS Signal Configuration getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); @@ -1246,35 +1253,21 @@ void UbloxFirmware8::getRosParams() { // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - bool temp; - nh->param("nmea/limit82", temp, false); - cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_LIMIT82 : 0; - nh->param("nmea/high_prec", temp, false); - cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + cfg_nmea_.flags |= getRosBoolean("nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean("nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; // set filter - nh->param("nmea/filter/pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0; - nh->param("nmea/filter/msk_pos", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0; - nh->param("nmea/filter/time", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0; - nh->param("nmea/filter/date", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0; - nh->param("nmea/filter/gps_only", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0; - nh->param("nmea/filter/track", temp, false); - cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - nh->param("nmea/gnssToFilter/gps", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - nh->param("nmea/gnssToFilter/sbas", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - nh->param("nmea/gnssToFilter/qzss", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - nh->param("nmea/gnssToFilter/glonass", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - nh->param("nmea/gnssToFilter/beidou", temp, false); - cfg_nmea_.gnss_to_filter |= temp ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); @@ -1286,7 +1279,6 @@ void UbloxFirmware8::getRosParams() { } } - bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { if (clear_bbr_) { // clear flash memory From bb9dddeee9891c75fd2b89b41135e20d1f475a91 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 13:45:01 +0000 Subject: [PATCH 061/152] Declare a few more booleans. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 99064b85..8f1c19ab 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -315,6 +315,9 @@ void UbloxNode::getRosParams() { declareRosBoolean("raw_data", false); declareRosBoolean("clear_bbr", false); declareRosBoolean("save_on_shutdown", false); + declareRosBoolean("use_adr", true); + + declareRosBoolean("sv_in/reset", true); // raw data stream logging rawDataStreamPa_.getRosParams(); @@ -1017,13 +1020,13 @@ void UbloxFirmware7::getRosParams() { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } - if (nh->hasParam("gnss/galileo")) { + if (getRosBoolean("gnss/galileo")) { ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); } - if (nh->hasParam("gnss/beidou")) { + if (getRosBoolean("gnss/beidou")) { ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); } - if (nh->hasParam("gnss/imes")) { + if (getRosBoolean("gnss/imes")) { ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); } @@ -1483,7 +1486,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s } void AdrUdrProduct::getRosParams() { - nh->param("use_adr", use_adr_, true); + use_adr_ = getRosBoolean("use_adr"); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { @@ -1673,7 +1676,7 @@ void HpgRefProduct::getRosParams() { lla_flag_ = false; } } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - nh->param("sv_in/reset", svin_reset_, true); + svin_reset_ = getRosBoolean("sv_in/reset"); if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + "must be set if TMODE3 is survey-in"); From 7b5e0bc6e0b08bce4a6fcdc04ab8f306d5fc6b53 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 13:56:50 +0000 Subject: [PATCH 062/152] Start passing the node into functions. This is the first step to making it not a global variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 70 ++-- ublox_gps/src/node.cpp | 484 +++++++++++++-------------- 2 files changed, 277 insertions(+), 277 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 6e3cbf7a..bc6bd32f 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -145,9 +145,9 @@ void checkRange(std::vector val, T min, T max, const std::string & name) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, U &u) { +bool getRosUint(ros::NodeHandle* node, const std::string& key, U &u) { int param; - if (!nh->getParam(key, param)) { + if (!node->getParam(key, param)) { return false; } // Check the bounds @@ -168,8 +168,8 @@ bool getRosUint(const std::string& key, U &u) { * @return true if found, false if not found. */ template -void getRosUint(const std::string& key, U &u, V default_val) { - if (!getRosUint(key, u)) { +void getRosUint(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { + if (!getRosUint(node, key, u)) { u = default_val; } } @@ -180,9 +180,9 @@ void getRosUint(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosUint(const std::string& key, std::vector &u) { +bool getRosUint(ros::NodeHandle* node, const std::string& key, std::vector &u) { std::vector param; - if (!nh->getParam(key, param)) { + if (!node->getParam(key, param)) { return false; } @@ -204,9 +204,9 @@ bool getRosUint(const std::string& key, std::vector &u) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, I &u) { +bool getRosInt(ros::NodeHandle* node, const std::string& key, I &u) { int param; - if (!nh->getParam(key, param)) { + if (!node->getParam(key, param)) { return false; } // Check the bounds @@ -227,8 +227,8 @@ bool getRosInt(const std::string& key, I &u) { * @return true if found, false if not found. */ template -void getRosInt(const std::string& key, U &u, V default_val) { - if (!getRosInt(key, u)) { +void getRosInt(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { + if (!getRosInt(node, key, u)) { u = default_val; } } @@ -239,9 +239,9 @@ void getRosInt(const std::string& key, U &u, V default_val) { * @return true if found, false if not found. */ template -bool getRosInt(const std::string& key, std::vector &i) { +bool getRosInt(ros::NodeHandle* node, const std::string& key, std::vector &i) { std::vector param; - if (!nh->getParam(key, param)) { + if (!node->getParam(key, param)) { return false; } @@ -255,24 +255,24 @@ bool getRosInt(const std::string& key, std::vector &i) { return true; } -bool declareRosBoolean(const std::string &name, bool default_value) +bool declareRosBoolean(ros::NodeHandle* node, const std::string &name, bool default_value) { bool ret; - if (!nh->hasParam(name)) { - nh->setParam(name, default_value); + if (!node->hasParam(name)) { + node->setParam(name, default_value); } // implicit else: If the ROS node already has the parameter, just leave it - if (!nh->getParam(name, ret)) { + if (!node->getParam(name, ret)) { throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); } } -bool getRosBoolean(const std::string &name) +bool getRosBoolean(ros::NodeHandle* node, const std::string &name) { bool ret; - if (!nh->getParam(name, ret)) { + if (!node->getParam(name, ret)) { // Note that if this is used after declareRosBoolean, this should never happen. throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); } @@ -611,16 +611,16 @@ class UbloxFirmware6 final : public UbloxFirmware { template class UbloxFirmware7Plus : public UbloxFirmware { public: - explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) : UbloxFirmware(updater, gnss), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher - nav_pvt_pub_ = nh->advertise("navpvt", 1); + nav_pvt_pub_ = node->advertise("navpvt", 1); fix_pub_ = - nh->advertise("fix", 1); + node->advertise("fix", 1); vel_pub_ = - nh->advertise("fix_velocity", - 1); + node->advertise("fix_velocity", + 1); } /** @@ -631,8 +631,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { * is published. This function also calls the ROS diagnostics updater. * @param m the message to publish */ - void callbackNavPvt(const NavPVT& m) { - if (getRosBoolean("publish/nav/pvt")) { + void callbackNavPvt(ros::NodeHandle* node, const NavPVT& m) { + if (getRosBoolean(node, "publish/nav/pvt")) { // NavPVT publisher nav_pvt_pub_.publish(m); } @@ -793,10 +793,10 @@ class UbloxFirmware7Plus : public UbloxFirmware { */ class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss) { - nav_svinfo_pub_ = nh->advertise("navsvinfo", 1); - mon_hw_pub_ = nh->advertise("monhw", 1); + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_svinfo_pub_ = node->advertise("navsvinfo", 1); + mon_hw_pub_ = node->advertise("monhw", 1); } /** @@ -834,11 +834,11 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { */ class UbloxFirmware8 : public UbloxFirmware7Plus { public: - explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss) { - nav_sat_pub_ = nh->advertise("navstate", 1); - mon_hw_pub_ = nh->advertise("monhw", 1); - rxm_rtcm_pub_ = nh->advertise("rxmrtcm", 1); + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_sat_pub_ = node->advertise("navstate", 1); + mon_hw_pub_ = node->advertise("monhw", 1); + rxm_rtcm_pub_ = node->advertise("rxmrtcm", 1); } /** @@ -892,7 +892,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ class UbloxFirmware9 final : public UbloxFirmware8 { public: - explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss); + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); }; /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 8f1c19ab..60b49c79 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -166,13 +166,13 @@ void UbloxNode::addFirmwareInterface() { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); ublox_version = 9; } @@ -207,56 +207,56 @@ void UbloxNode::getRosParams() { nh->param("frame_id", frame_id_, std::string("gps")); // Save configuration parameters - getRosUint("load/mask", load_.load_mask, 0); - getRosUint("load/device", load_.device_mask, 0); - getRosUint("save/mask", save_.save_mask, 0); - getRosUint("save/device", save_.device_mask, 0); + getRosUint(nh.get(), "load/mask", load_.load_mask, 0); + getRosUint(nh.get(), "load/device", load_.device_mask, 0); + getRosUint(nh.get(), "save/mask", save_.save_mask, 0); + getRosUint(nh.get(), "save/device", save_.device_mask, 0); // UART 1 params - getRosUint("uart1/baudrate", baudrate_, 9600); - getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX + getRosUint(nh.get(), "uart1/baudrate", baudrate_, 9600); + getRosUint(nh.get(), "uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX | ublox_msgs::CfgPRT::PROTO_NMEA | ublox_msgs::CfgPRT::PROTO_RTCM); - getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); + getRosUint(nh.get(), "uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { set_usb_ = true; - if (!getRosUint("usb/in", usb_in_)) { + if (!getRosUint(nh.get(), "usb/in", usb_in_)) { throw std::runtime_error(std::string("usb/out is set, therefore ") + "usb/in must be set"); } - if (!getRosUint("usb/out", usb_out_)) { + if (!getRosUint(nh.get(), "usb/out", usb_out_)) { throw std::runtime_error(std::string("usb/in is set, therefore ") + "usb/out must be set"); } - getRosUint("usb/tx_ready", usb_tx_, 0); + getRosUint(nh.get(), "usb/tx_ready", usb_tx_, 0); } // Measurement rate params nh->param("rate", rate_, 4.0); // in Hz - getRosUint("nav_rate", nav_rate_, 1); // # of measurement rate cycles + getRosUint(nh.get(), "nav_rate", nav_rate_, 1); // # of measurement rate cycles // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; - getRosUint("rtcm/ids", rtcm_ids); // RTCM output message IDs - getRosUint("rtcm/rates", rtcm_rates); // RTCM output message rates + getRosUint(nh.get(), "rtcm/ids", rtcm_ids); // RTCM output message IDs + getRosUint(nh.get(), "rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting - declareRosBoolean("enable_ppp", false); + declareRosBoolean(nh.get(), "enable_ppp", false); // SBAS params, only for some devices - declareRosBoolean("gnss/sbas", false); - declareRosBoolean("gnss/gps", true); - declareRosBoolean("gnss/glonass", false); - declareRosBoolean("gnss/qzss", false); - declareRosBoolean("gnss/galileo", false); - declareRosBoolean("gnss/beidou", false); - declareRosBoolean("gnss/imes", false); - getRosUint("sbas/max", max_sbas_, 0); // Maximum number of SBAS channels - getRosUint("sbas/usage", sbas_usage_, 0); + declareRosBoolean(nh.get(), "gnss/sbas", false); + declareRosBoolean(nh.get(), "gnss/gps", true); + declareRosBoolean(nh.get(), "gnss/glonass", false); + declareRosBoolean(nh.get(), "gnss/qzss", false); + declareRosBoolean(nh.get(), "gnss/galileo", false); + declareRosBoolean(nh.get(), "gnss/beidou", false); + declareRosBoolean(nh.get(), "gnss/imes", false); + getRosUint(nh.get(), "sbas/max", max_sbas_, 0); // Maximum number of SBAS channels + getRosUint(nh.get(), "sbas/usage", sbas_usage_, 0); nh->param("dynamic_model", dynamic_model_, std::string("portable")); nh->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint("dr_limit", dr_limit_, 0); // Dead reckoning limit + getRosUint(nh.get(), "dr_limit", dr_limit_, 0); // Dead reckoning limit - if (getRosBoolean("enable_ppp")) { + if (getRosBoolean(nh.get(), "enable_ppp")) { ROS_WARN("Warning: PPP is enabled - this is an expert setting."); } @@ -276,8 +276,8 @@ void UbloxNode::getRosParams() { dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); - declareRosBoolean("dat/set", false); - if (getRosBoolean("dat/set")) { + declareRosBoolean(nh.get(), "dat/set", false); + if (getRosBoolean(nh.get(), "dat/set")) { std::vector shift, rot; if (!nh->getParam("dat/majA", cfg_dat_.maj_a) || nh->getParam("dat/flat", cfg_dat_.flat) @@ -311,99 +311,99 @@ void UbloxNode::getRosParams() { meas_rate_ = 1000 / rate_; // activate/deactivate any config - declareRosBoolean("config_on_startup", true); - declareRosBoolean("raw_data", false); - declareRosBoolean("clear_bbr", false); - declareRosBoolean("save_on_shutdown", false); - declareRosBoolean("use_adr", true); + declareRosBoolean(nh.get(), "config_on_startup", true); + declareRosBoolean(nh.get(), "raw_data", false); + declareRosBoolean(nh.get(), "clear_bbr", false); + declareRosBoolean(nh.get(), "save_on_shutdown", false); + declareRosBoolean(nh.get(), "use_adr", true); - declareRosBoolean("sv_in/reset", true); + declareRosBoolean(nh.get(), "sv_in/reset", true); // raw data stream logging rawDataStreamPa_.getRosParams(); // NMEA parameters - declareRosBoolean("nmea/set", false); - declareRosBoolean("nmea/compat", false); - declareRosBoolean("nmea/consider", false); - declareRosBoolean("nmea/limit82", false); - declareRosBoolean("nmea/high_prec", false); - declareRosBoolean("nmea/filter/pos", false); - declareRosBoolean("nmea/filter/msk_pos", false); - declareRosBoolean("nmea/filter/time", false); - declareRosBoolean("nmea/filter/date", false); - declareRosBoolean("nmea/filter/sbas", false); - declareRosBoolean("nmea/filter/track", false); - declareRosBoolean("nmea/filter/gps_only", false); - declareRosBoolean("nmea/gnssToFilter/gps", false); - declareRosBoolean("nmea/gnssToFilter/sbas", false); - declareRosBoolean("nmea/gnssToFilter/qzss", false); - declareRosBoolean("nmea/gnssToFilter/glonass", false); - declareRosBoolean("nmea/gnssToFilter/beidou", false); + declareRosBoolean(nh.get(), "nmea/set", false); + declareRosBoolean(nh.get(), "nmea/compat", false); + declareRosBoolean(nh.get(), "nmea/consider", false); + declareRosBoolean(nh.get(), "nmea/limit82", false); + declareRosBoolean(nh.get(), "nmea/high_prec", false); + declareRosBoolean(nh.get(), "nmea/filter/pos", false); + declareRosBoolean(nh.get(), "nmea/filter/msk_pos", false); + declareRosBoolean(nh.get(), "nmea/filter/time", false); + declareRosBoolean(nh.get(), "nmea/filter/date", false); + declareRosBoolean(nh.get(), "nmea/filter/sbas", false); + declareRosBoolean(nh.get(), "nmea/filter/track", false); + declareRosBoolean(nh.get(), "nmea/filter/gps_only", false); + declareRosBoolean(nh.get(), "nmea/gnssToFilter/gps", false); + declareRosBoolean(nh.get(), "nmea/gnssToFilter/sbas", false); + declareRosBoolean(nh.get(), "nmea/gnssToFilter/qzss", false); + declareRosBoolean(nh.get(), "nmea/gnssToFilter/glonass", false); + declareRosBoolean(nh.get(), "nmea/gnssToFilter/beidou", false); // Publish parameters - declareRosBoolean("publish/all", false); - - declareRosBoolean("publish/nav/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/nav/att", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/clock", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/heading", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/posecef", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/posllh", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/pvt", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/relposned", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/sat", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/sol", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/svin", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/svinfo", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/status", getRosBoolean("publish/nav/all")); - declareRosBoolean("publish/nav/velned", getRosBoolean("publish/nav/all")); - - declareRosBoolean("publish/rxm/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/rxm/almRaw", getRosBoolean("publish/rxm/all")); - declareRosBoolean("publish/rxm/eph", getRosBoolean("publish/rxm/all")); - declareRosBoolean("publish/rxm/rtcm", getRosBoolean("publish/rxm/all")); - declareRosBoolean("publish/rxm/raw", getRosBoolean("publish/rxm/all")); - declareRosBoolean("publish/rxm/sfrb", getRosBoolean("publish/rxm/all")); - - declareRosBoolean("publish/aid/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/aid/alm", getRosBoolean("publish/aid/all")); - declareRosBoolean("publish/aid/eph", getRosBoolean("publish/aid/all")); - declareRosBoolean("publish/aid/hui", getRosBoolean("publish/aid/all")); - - declareRosBoolean("publish/mon/all", getRosBoolean("publish/all")); - declareRosBoolean("publish/mon/hw", getRosBoolean("publish/mon/all")); - - declareRosBoolean("publish/tim/tm2", false); + declareRosBoolean(nh.get(), "publish/all", false); + + declareRosBoolean(nh.get(), "publish/nav/all", getRosBoolean(nh.get(), "publish/all")); + declareRosBoolean(nh.get(), "publish/nav/att", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/clock", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/heading", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/posecef", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/posllh", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/pvt", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/relposned", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/sat", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/sol", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/svin", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/svinfo", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/status", getRosBoolean(nh.get(), "publish/nav/all")); + declareRosBoolean(nh.get(), "publish/nav/velned", getRosBoolean(nh.get(), "publish/nav/all")); + + declareRosBoolean(nh.get(), "publish/rxm/all", getRosBoolean(nh.get(), "publish/all")); + declareRosBoolean(nh.get(), "publish/rxm/almRaw", getRosBoolean(nh.get(), "publish/rxm/all")); + declareRosBoolean(nh.get(), "publish/rxm/eph", getRosBoolean(nh.get(), "publish/rxm/all")); + declareRosBoolean(nh.get(), "publish/rxm/rtcm", getRosBoolean(nh.get(), "publish/rxm/all")); + declareRosBoolean(nh.get(), "publish/rxm/raw", getRosBoolean(nh.get(), "publish/rxm/all")); + declareRosBoolean(nh.get(), "publish/rxm/sfrb", getRosBoolean(nh.get(), "publish/rxm/all")); + + declareRosBoolean(nh.get(), "publish/aid/all", getRosBoolean(nh.get(), "publish/all")); + declareRosBoolean(nh.get(), "publish/aid/alm", getRosBoolean(nh.get(), "publish/aid/all")); + declareRosBoolean(nh.get(), "publish/aid/eph", getRosBoolean(nh.get(), "publish/aid/all")); + declareRosBoolean(nh.get(), "publish/aid/hui", getRosBoolean(nh.get(), "publish/aid/all")); + + declareRosBoolean(nh.get(), "publish/mon/all", getRosBoolean(nh.get(), "publish/all")); + declareRosBoolean(nh.get(), "publish/mon/hw", getRosBoolean(nh.get(), "publish/mon/all")); + + declareRosBoolean(nh.get(), "publish/tim/tm2", false); // INF parameters - declareRosBoolean("inf/all", true); - declareRosBoolean("inf/debug", false); - declareRosBoolean("inf/error", getRosBoolean("inf/all")); - declareRosBoolean("inf/notice", getRosBoolean("inf/all")); - declareRosBoolean("inf/test", getRosBoolean("inf/all")); - declareRosBoolean("inf/warning", getRosBoolean("inf/all")); + declareRosBoolean(nh.get(), "inf/all", true); + declareRosBoolean(nh.get(), "inf/debug", false); + declareRosBoolean(nh.get(), "inf/error", getRosBoolean(nh.get(), "inf/all")); + declareRosBoolean(nh.get(), "inf/notice", getRosBoolean(nh.get(), "inf/all")); + declareRosBoolean(nh.get(), "inf/test", getRosBoolean(nh.get(), "inf/all")); + declareRosBoolean(nh.get(), "inf/warning", getRosBoolean(nh.get(), "inf/all")); // ESF parameters - declareRosBoolean("publish/esf/all", true); - declareRosBoolean("publish/esf/ins", getRosBoolean("publish/esf/all")); - declareRosBoolean("publish/esf/meas", getRosBoolean("publish/esf/all")); - declareRosBoolean("publish/esf/raw", getRosBoolean("publish/esf/all")); - declareRosBoolean("publish/esf/status", getRosBoolean("publish/esf/all")); + declareRosBoolean(nh.get(), "publish/esf/all", true); + declareRosBoolean(nh.get(), "publish/esf/ins", getRosBoolean(nh.get(), "publish/esf/all")); + declareRosBoolean(nh.get(), "publish/esf/meas", getRosBoolean(nh.get(), "publish/esf/all")); + declareRosBoolean(nh.get(), "publish/esf/raw", getRosBoolean(nh.get(), "publish/esf/all")); + declareRosBoolean(nh.get(), "publish/esf/status", getRosBoolean(nh.get(), "publish/esf/all")); // HNR parameters - declareRosBoolean("publish/hnr/pvt", true); + declareRosBoolean(nh.get(), "publish/hnr/pvt", true); } void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); - if (getRosBoolean("publish/aid/alm")) { + if (getRosBoolean(nh.get(), "publish/aid/alm")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } - if (getRosBoolean("publish/aid/eph")) { + if (getRosBoolean(nh.get(), "publish/aid/eph")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } - if (getRosBoolean("publish/aid/hui")) { + if (getRosBoolean(nh.get(), "publish/aid/hui")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } @@ -430,51 +430,51 @@ void UbloxNode::subscribe() { // subscribe messages // Nav Messages - if (getRosBoolean("publish/nav/status")) { + if (getRosBoolean(nh.get(), "publish/nav/status")) { gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, 1); } - if (getRosBoolean("publish/nav/posecef")) { + if (getRosBoolean(nh.get(), "publish/nav/posecef")) { gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, 1); } - if (getRosBoolean("publish/nav/clock")) { + if (getRosBoolean(nh.get(), "publish/nav/clock")) { gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, 1); } // INF messages - if (getRosBoolean("inf/debug")) { + if (getRosBoolean(nh.get(), "inf/debug")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); } - if (getRosBoolean("inf/error")) { + if (getRosBoolean(nh.get(), "inf/error")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); } - if (getRosBoolean("inf/notice")) { + if (getRosBoolean(nh.get(), "inf/notice")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); } - if (getRosBoolean("inf/test")) { + if (getRosBoolean(nh.get(), "inf/test")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); } - if (getRosBoolean("inf/warning")) { + if (getRosBoolean(nh.get(), "inf/warning")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), @@ -482,17 +482,17 @@ void UbloxNode::subscribe() { } // AID messages - if (getRosBoolean("publish/aid/alm")) { + if (getRosBoolean(nh.get(), "publish/aid/alm")) { gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, 1); } - if (getRosBoolean("publish/aid/eph")) { + if (getRosBoolean(nh.get(), "publish/aid/eph")) { gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, 1); } - if (getRosBoolean("publish/aid/hui")) { + if (getRosBoolean(nh.get(), "publish/aid/hui")) { gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, 1); } @@ -503,7 +503,7 @@ void UbloxNode::subscribe() { } void UbloxNode::initializeRosDiagnostics() { - declareRosBoolean("diagnostic_period", kDiagnosticPeriod); + declareRosBoolean(nh.get(), "diagnostic_period", kDiagnosticPeriod); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); @@ -603,7 +603,7 @@ bool UbloxNode::configureUblox() { } } - if (getRosBoolean("config_on_startup")) { + if (getRosBoolean(nh.get(), "config_on_startup")) { if (set_usb_) { gps_->configUsb(usb_tx_, usb_in_, usb_out_); } @@ -615,15 +615,15 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { - if (!gps_->configSbas(getRosBoolean("gnss/sbas"), sbas_usage_, max_sbas_)) { + if (!gps_->configSbas(getRosBoolean(nh.get(), "gnss/sbas"), sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean("gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(nh.get(), "gnss/sbas") ? "enable" : "disable") + " SBAS."); } } - if (!gps_->setPpp(getRosBoolean("enable_ppp"))) { + if (!gps_->setPpp(getRosBoolean(nh.get(), "enable_ppp"))) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean("enable_ppp") ? "enable" : "disable") + (getRosBoolean(nh.get(), "enable_ppp") ? "enable" : "disable") + " PPP."); } if (!gps_->setDynamicModel(dmodel_)) { @@ -637,7 +637,7 @@ bool UbloxNode::configureUblox() { ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (getRosBoolean("dat/set") && !gps_->configure(cfg_dat_)) { + if (getRosBoolean(nh.get(), "dat/set") && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component @@ -667,11 +667,11 @@ void UbloxNode::configureInf() { ublox_msgs::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (getRosBoolean("inf/error") ? block.INF_MSG_ERROR : 0) | - (getRosBoolean("inf/warning") ? block.INF_MSG_WARNING : 0) | - (getRosBoolean("inf/notice") ? block.INF_MSG_NOTICE : 0) | - (getRosBoolean("inf/test") ? block.INF_MSG_TEST : 0) | - (getRosBoolean("inf/debug") ? block.INF_MSG_DEBUG : 0); + uint8_t mask = (getRosBoolean(nh.get(), "inf/error") ? block.INF_MSG_ERROR : 0) | + (getRosBoolean(nh.get(), "inf/warning") ? block.INF_MSG_WARNING : 0) | + (getRosBoolean(nh.get(), "inf/notice") ? block.INF_MSG_NOTICE : 0) | + (getRosBoolean(nh.get(), "inf/test") ? block.INF_MSG_TEST : 0) | + (getRosBoolean(nh.get(), "inf/debug") ? block.INF_MSG_DEBUG : 0); for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } @@ -696,7 +696,7 @@ void UbloxNode::configureInf() { } void UbloxNode::initializeIo() { - gps_->setConfigOnStartup(getRosBoolean("config_on_startup")); + gps_->setConfigOnStartup(getRosBoolean(nh.get(), "config_on_startup")); std::smatch match; if (std::regex_match(device_, match, @@ -730,7 +730,7 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) { - if (getRosBoolean("raw_data")) { + if (getRosBoolean(nh.get(), "raw_data")) { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_)); } } @@ -809,37 +809,37 @@ void UbloxFirmware6::getRosParams() { // Fix Service type, used when publishing fix status messages fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; - if (getRosBoolean("nmea/set")) { + if (getRosBoolean(nh.get(), "nmea/set")) { bool compat, consider; - if (!getRosUint("nmea/version", cfg_nmea_.version)) { + if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } // set flags - cfg_nmea_.flags = getRosBoolean("nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= getRosBoolean("nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = getRosBoolean(nh.get(), "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter bool temp; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; } } bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -859,13 +859,13 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean("publish/nav/svinfo")) { + if (getRosBoolean(nh.get(), "publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean("publish/mon/hw")) { + if (getRosBoolean(nh.get(), "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, 1); } @@ -914,7 +914,7 @@ void UbloxFirmware6::fixDiagnostic( } void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if (getRosBoolean("publish/nav/posllh")) { + if (getRosBoolean(nh.get(), "publish/nav/posllh")) { nav_pos_llh_pub_.publish(m); } @@ -955,7 +955,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if (getRosBoolean("publish/nav/velned")) { + if (getRosBoolean(nh.get(), "publish/nav/velned")) { nav_vel_ned_pub_.publish(m); } @@ -985,7 +985,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { } void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if (getRosBoolean("publish/nav/sol")) { + if (getRosBoolean(nh.get(), "publish/nav/sol")) { nav_sol_pub_.publish(m); } last_nav_sol_ = m; @@ -999,11 +999,11 @@ void UbloxFirmware7::getRosParams() { // GNSS configuration // // GNSS enable/disable - enable_gps_ = getRosBoolean("gnss/gps"); - enable_glonass_ = getRosBoolean("gnss/glonass"); - enable_qzss_ = getRosBoolean("gnss/qzss"); + enable_gps_ = getRosBoolean(nh.get(), "gnss/gps"); + enable_glonass_ = getRosBoolean(nh.get(), "gnss/glonass"); + enable_qzss_ = getRosBoolean(nh.get(), "gnss/qzss"); - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, + getRosUint(nh.get(), "gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { @@ -1016,17 +1016,17 @@ void UbloxFirmware7::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean("gnss/sbas") && !gnss_->isSupported("SBAS")) { + if (getRosBoolean(nh.get(), "gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } - if (getRosBoolean("gnss/galileo")) { + if (getRosBoolean(nh.get(), "gnss/galileo")) { ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); } - if (getRosBoolean("gnss/beidou")) { + if (getRosBoolean(nh.get(), "gnss/beidou")) { ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); } - if (getRosBoolean("gnss/imes")) { + if (getRosBoolean(nh.get(), "gnss/imes")) { ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); } @@ -1037,18 +1037,18 @@ void UbloxFirmware7::getRosParams() { // // NMEA Configuration // - if (getRosBoolean("nmea/set")) { + if (getRosBoolean(nh.get(), "nmea/set")) { bool compat, consider; - if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) { + if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + if (!getRosUint(nh.get(), "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); } @@ -1065,20 +1065,20 @@ void UbloxFirmware7::getRosParams() { cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(nh.get(), "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(nh.get(), "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); } } @@ -1135,16 +1135,16 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { block.gnss_id = block.GNSS_ID_SBAS; block.res_trk_ch = block.RES_TRK_CH_SBAS; block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = getRosBoolean("gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; + block.flags = getRosBoolean(nh.get(), "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean("gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(nh.get(), "gnss/sbas") ? "enable" : "disable") + " SBAS."); } } - if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1155,17 +1155,17 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT (always does so since fix information is published // from this) gps->subscribe(std::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), + &UbloxFirmware7Plus::callbackNavPvt, this, nh.get(), std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean("publish/nav/svinfo")) { + if (getRosBoolean(nh.get(), "publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean("publish/mon/hw")) { + if (getRosBoolean(nh.get(), "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } @@ -1176,19 +1176,19 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // void UbloxFirmware8::getRosParams() { // UPD SOS configuration - clear_bbr_ = getRosBoolean("clear_bbr"); - save_on_shutdown_ = getRosBoolean("save_on_shutdown"); + clear_bbr_ = getRosBoolean(nh.get(), "clear_bbr"); + save_on_shutdown_ = getRosBoolean(nh.get(), "save_on_shutdown"); // GNSS enable/disable - enable_gps_ = getRosBoolean("gnss/gps"); - enable_galileo_ = getRosBoolean("gnss/galileo"); - enable_beidou_ = getRosBoolean("gnss/beidou"); - enable_imes_ = getRosBoolean("gnss/imes"); - enable_glonass_ = getRosBoolean("gnss/glonass"); - enable_qzss_ = getRosBoolean("gnss/qzss"); + enable_gps_ = getRosBoolean(nh.get(), "gnss/gps"); + enable_galileo_ = getRosBoolean(nh.get(), "gnss/galileo"); + enable_beidou_ = getRosBoolean(nh.get(), "gnss/beidou"); + enable_imes_ = getRosBoolean(nh.get(), "gnss/imes"); + enable_glonass_ = getRosBoolean(nh.get(), "gnss/glonass"); + enable_qzss_ = getRosBoolean(nh.get(), "gnss/qzss"); // QZSS Signal Configuration - getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_, + getRosUint(nh.get(), "gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { @@ -1214,7 +1214,7 @@ void UbloxFirmware8::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean("gnss/sbas") && !gnss_->isSupported("SBAS")) { + if (getRosBoolean(nh.get(), "gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1227,20 +1227,20 @@ void UbloxFirmware8::getRosParams() { // // NMEA Configuration // - if (getRosBoolean("nmea/set")) { + if (getRosBoolean(nh.get(), "nmea/set")) { bool compat, consider; cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set - if (!getRosUint("nmea/version", cfg_nmea_.nmea_version)) { + if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint("nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } - if (!getRosUint("nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + if (!getRosUint(nh.get(), "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); } @@ -1256,27 +1256,27 @@ void UbloxFirmware8::getRosParams() { // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - cfg_nmea_.flags |= getRosBoolean("nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; - cfg_nmea_.flags |= getRosBoolean("nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; // set filter - cfg_nmea_.filter |= getRosBoolean("nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean("nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean("nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - getRosUint("nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(nh.get(), "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(nh.get(), "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); std::vector bds_talker_id; - getRosUint("nmea/bds_talker_id", bds_talker_id); + getRosUint(nh.get(), "nmea/bds_talker_id", bds_talker_id); cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; } @@ -1316,10 +1316,10 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_SBAS - && getRosBoolean("gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { + && getRosBoolean(nh.get(), "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean("gnss/sbas"); + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(nh.get(), "gnss/sbas"); ROS_DEBUG("SBAS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { @@ -1375,7 +1375,7 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // // NMEA config // - if (getRosBoolean("nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1385,29 +1385,29 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT gps->subscribe( - std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, nh.get(), std::placeholders::_1), 1); // Subscribe to Nav SAT messages - if (getRosBoolean("publish/nav/sat")) { + if (getRosBoolean(nh.get(), "publish/nav/sat")) { gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean("publish/mon/hw")) { + if (getRosBoolean(nh.get(), "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } // Subscribe to RTCM messages - if (getRosBoolean("publish/rxm/rtcm")) { + if (getRosBoolean(nh.get(), "publish/rxm/rtcm")) { gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, 1); } } -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) - : UbloxFirmware8(frame_id, updater, freq_diag, gnss) +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) { } @@ -1424,44 +1424,44 @@ RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::share void RawDataProduct::subscribe(std::shared_ptr gps) { // Subscribe to RXM Raw - if (getRosBoolean("publish/rxm/raw")) { + if (getRosBoolean(nh.get(), "publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, 1); } // Subscribe to RXM SFRB - if (getRosBoolean("publish/rxm/sfrb")) { + if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RXM EPH - if (getRosBoolean("publish/rxm/eph")) { + if (getRosBoolean(nh.get(), "publish/rxm/eph")) { gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, 1); } // Subscribe to RXM ALM - if (getRosBoolean("publish/rxm/almRaw")) { + if (getRosBoolean(nh.get(), "publish/rxm/almRaw")) { gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, 1); } } void RawDataProduct::initializeRosDiagnostics() { - if (getRosBoolean("publish/rxm/raw")) { + if (getRosBoolean(nh.get(), "publish/rxm/raw")) { freq_diagnostics_.push_back(std::make_shared( "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean("publish/rxm/sfrb")) { + if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { freq_diagnostics_.push_back(std::make_shared( "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean("publish/rxm/eph")) { + if (getRosBoolean(nh.get(), "publish/rxm/eph")) { freq_diagnostics_.push_back(std::make_shared( "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean("publish/rxm/almRaw")) { + if (getRosBoolean(nh.get(), "publish/rxm/almRaw")) { freq_diagnostics_.push_back(std::make_shared( "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } @@ -1486,7 +1486,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s } void AdrUdrProduct::getRosParams() { - use_adr_ = getRosBoolean("use_adr"); + use_adr_ = getRosBoolean(nh.get(), "use_adr"); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { @@ -1504,19 +1504,19 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to NAV ATT messages - if (getRosBoolean("publish/nav/att")) { + if (getRosBoolean(nh.get(), "publish/nav/att")) { gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, 1); } // Subscribe to ESF INS messages - if (getRosBoolean("publish/esf/ins")) { + if (getRosBoolean(nh.get(), "publish/esf/ins")) { gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, 1); } // Subscribe to ESF Meas messages - if (getRosBoolean("publish/esf/meas")) { + if (getRosBoolean(nh.get(), "publish/esf/meas")) { gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, 1); // also publish sensor_msgs::Imu @@ -1525,26 +1525,26 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { } // Subscribe to ESF Raw messages - if (getRosBoolean("publish/esf/raw")) { + if (getRosBoolean(nh.get(), "publish/esf/raw")) { gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, 1); } // Subscribe to ESF Status messages - if (getRosBoolean("publish/esf/status")) { + if (getRosBoolean(nh.get(), "publish/esf/status")) { gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, 1); } // Subscribe to HNR PVT messages - if (getRosBoolean("publish/hnr/pvt")) { + if (getRosBoolean(nh.get(), "publish/hnr/pvt")) { gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, 1); } } void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (getRosBoolean("publish/esf/meas")) { + if (getRosBoolean(nh.get(), "publish/esf/meas")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -1648,12 +1648,12 @@ HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ } void HpgRefProduct::getRosParams() { - if (getRosBoolean("config_on_startup")) { + if (getRosBoolean(nh.get(), "config_on_startup")) { if (nav_rate_ * meas_rate_ != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); } - if (!getRosUint("tmode3", tmode3_)) { + if (!getRosUint(nh.get(), "tmode3", tmode3_)) { throw std::runtime_error("Invalid settings: TMODE3 must be set"); } @@ -1662,7 +1662,7 @@ void HpgRefProduct::getRosParams() { throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); } - if (!getRosInt("arp/position_hp", arp_position_hp_)) { + if (!getRosInt(nh.get(), "arp/position_hp", arp_position_hp_)) { throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); } @@ -1676,8 +1676,8 @@ void HpgRefProduct::getRosParams() { lla_flag_ = false; } } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - svin_reset_ = getRosBoolean("sv_in/reset"); - if (!getRosUint("sv_in/min_dur", sv_in_min_dur_)) { + svin_reset_ = getRosBoolean(nh.get(), "sv_in/reset"); + if (!getRosUint(nh.get(), "sv_in/min_dur", sv_in_min_dur_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + "must be set if TMODE3 is survey-in"); } @@ -1773,7 +1773,7 @@ void HpgRefProduct::subscribe(std::shared_ptr gps) { } void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { - if (getRosBoolean("publish/nav/svin")) { + if (getRosBoolean(nh.get(), "publish/nav/svin")) { navsvin_pub_.publish(m); } @@ -1863,8 +1863,8 @@ HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr gps) { @@ -1925,7 +1925,7 @@ void HpgRovProduct::carrierPhaseDiagnostics( } void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (getRosBoolean("publish/nav/relposned")) { + if (getRosBoolean(nh.get(), "publish/nav/relposned")) { nav_rel_pos_ned_pub_.publish(m); } @@ -1954,11 +1954,11 @@ void HpPosRecProduct::subscribe(std::shared_ptr gps) { } void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (getRosBoolean("publish/nav/relposned")) { + if (getRosBoolean(nh.get(), "publish/nav/relposned")) { nav_relposned_pub_.publish(m); } - if (getRosBoolean("publish/nav/heading")) { + if (getRosBoolean(nh.get(), "publish/nav/heading")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -2021,13 +2021,13 @@ void TimProduct::subscribe(std::shared_ptr gps) { ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages - if (getRosBoolean("publish/rxm/sfrb")) { + if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RawX messages - if (getRosBoolean("publish/rxm/raw")) { + if (getRosBoolean(nh.get(), "publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, 1); } @@ -2035,7 +2035,7 @@ void TimProduct::subscribe(std::shared_ptr gps) { void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - if (getRosBoolean("publish/tim/tm2")) { + if (getRosBoolean(nh.get(), "publish/tim/tm2")) { // create time ref message and put in the data t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); From 4bb8c63c1292ad3e33fd19cac390613855b2fca2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:09:22 +0000 Subject: [PATCH 063/152] Pass the nodehandle into more methods. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 23 ++- ublox_gps/src/node.cpp | 270 +++++++++++++-------------- 2 files changed, 148 insertions(+), 145 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index bc6bd32f..58d336c5 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -496,7 +496,7 @@ class UbloxFirmware : public virtual ComponentInterface { //! Subscribe Rate for u-blox SV Info messages constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss); + explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node); /** * @brief Add the fix diagnostics to the updater. @@ -515,6 +515,7 @@ class UbloxFirmware : public virtual ComponentInterface { //! The fix status service type, set in the Firmware Component //! based on the enabled GNSS int fix_status_service_{0}; + ros::NodeHandle* node_; }; /** @@ -522,7 +523,7 @@ class UbloxFirmware : public virtual ComponentInterface { */ class UbloxFirmware6 final : public UbloxFirmware { public: - explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss); + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); /** * @brief Sets the fix status service type to GPS. @@ -612,14 +613,14 @@ template class UbloxFirmware7Plus : public UbloxFirmware { public: explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware(updater, gnss), frame_id_(frame_id), freq_diag_(freq_diag) { + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher - nav_pvt_pub_ = node->advertise("navpvt", 1); + nav_pvt_pub_ = node_->advertise("navpvt", 1); fix_pub_ = - node->advertise("fix", 1); + node_->advertise("fix", 1); vel_pub_ = - node->advertise("fix_velocity", + node_->advertise("fix_velocity", 1); } @@ -631,8 +632,8 @@ class UbloxFirmware7Plus : public UbloxFirmware { * is published. This function also calls the ROS diagnostics updater. * @param m the message to publish */ - void callbackNavPvt(ros::NodeHandle* node, const NavPVT& m) { - if (getRosBoolean(node, "publish/nav/pvt")) { + void callbackNavPvt(const NavPVT& m) { + if (getRosBoolean(node_, "publish/nav/pvt")) { // NavPVT publisher nav_pvt_pub_.publish(m); } @@ -903,7 +904,7 @@ class RawDataProduct final : public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; - explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater); + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node); /** * @brief Does nothing since there are no Raw Data product specific settings. @@ -943,6 +944,7 @@ class RawDataProduct final : public virtual ComponentInterface { uint16_t nav_rate_; uint16_t meas_rate_; std::shared_ptr updater_; + ros::NodeHandle* node_; }; /** @@ -951,7 +953,7 @@ class RawDataProduct final : public virtual ComponentInterface { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); /** * @brief Get the ADR/UDR parameters. @@ -1008,6 +1010,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { std::string frame_id_; std::shared_ptr updater_; + ros::NodeHandle* node_; }; /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 60b49c79..f3f11483 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -163,7 +163,7 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_)); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); @@ -192,7 +192,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(frame_id_, updater_)); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, nh.get())); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -731,7 +731,7 @@ void UbloxNode::initialize() { processMonVer(); if (protocol_version_ <= 14) { if (getRosBoolean(nh.get(), "raw_data")) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, nh.get())); } } // Must set firmware & hardware params before initializing diagnostics @@ -768,7 +768,7 @@ void UbloxNode::shutdown() { // // U-Blox Firmware (all versions) // -UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss) : updater_(updater), gnss_(gnss) +UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node) : updater_(updater), gnss_(gnss), node_(node) { } @@ -780,66 +780,65 @@ void UbloxFirmware::initializeRosDiagnostics() { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss) - : UbloxFirmware(updater, gnss), frame_id_(frame_id), freq_diag_(freq_diag) +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { nav_pos_llh_pub_ = - nh->advertise("navposllh", 1); + node_->advertise("navposllh", 1); fix_pub_ = - nh->advertise("fix", 1); + node_->advertise("fix", 1); nav_vel_ned_pub_ = - nh->advertise("navvelned", 1); + node_->advertise("navvelned", 1); vel_pub_ = - nh->advertise("fix_velocity", + node_->advertise("fix_velocity", 1); nav_sol_pub_ = - nh->advertise("navsol", 1); + node_->advertise("navsol", 1); nav_svinfo_pub_ = - nh->advertise("navinfo", 1); + node_->advertise("navinfo", 1); mon_hw_pub_ = - nh->advertise("monhw", 1); + node_->advertise("monhw", 1); } void UbloxFirmware6::getRosParams() { // Fix Service type, used when publishing fix status messages fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; - if (getRosBoolean(nh.get(), "nmea/set")) { + if (getRosBoolean(node_, "nmea/set")) { bool compat, consider; - if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.version)) { + if (!getRosUint(node_, "nmea/version", cfg_nmea_.version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } // set flags - cfg_nmea_.flags = getRosBoolean(nh.get(), "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = getRosBoolean(node_, "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - bool temp; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; } } bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -859,13 +858,13 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean(nh.get(), "publish/nav/svinfo")) { + if (getRosBoolean(node_, "publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(nh.get(), "publish/mon/hw")) { + if (getRosBoolean(node_, "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, 1); } @@ -914,7 +913,7 @@ void UbloxFirmware6::fixDiagnostic( } void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if (getRosBoolean(nh.get(), "publish/nav/posllh")) { + if (getRosBoolean(node_, "publish/nav/posllh")) { nav_pos_llh_pub_.publish(m); } @@ -955,7 +954,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if (getRosBoolean(nh.get(), "publish/nav/velned")) { + if (getRosBoolean(node_, "publish/nav/velned")) { nav_vel_ned_pub_.publish(m); } @@ -985,7 +984,7 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { } void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if (getRosBoolean(nh.get(), "publish/nav/sol")) { + if (getRosBoolean(node_, "publish/nav/sol")) { nav_sol_pub_.publish(m); } last_nav_sol_ = m; @@ -999,11 +998,11 @@ void UbloxFirmware7::getRosParams() { // GNSS configuration // // GNSS enable/disable - enable_gps_ = getRosBoolean(nh.get(), "gnss/gps"); - enable_glonass_ = getRosBoolean(nh.get(), "gnss/glonass"); - enable_qzss_ = getRosBoolean(nh.get(), "gnss/qzss"); + enable_gps_ = getRosBoolean(node_, "gnss/gps"); + enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); - getRosUint(nh.get(), "gnss/qzss_sig_cfg", qzss_sig_cfg_, + getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { @@ -1016,17 +1015,17 @@ void UbloxFirmware7::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean(nh.get(), "gnss/sbas") && !gnss_->isSupported("SBAS")) { + if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } - if (getRosBoolean(nh.get(), "gnss/galileo")) { + if (getRosBoolean(node_, "gnss/galileo")) { ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); } - if (getRosBoolean(nh.get(), "gnss/beidou")) { + if (getRosBoolean(node_, "gnss/beidou")) { ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); } - if (getRosBoolean(nh.get(), "gnss/imes")) { + if (getRosBoolean(node_, "gnss/imes")) { ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); } @@ -1037,26 +1036,26 @@ void UbloxFirmware7::getRosParams() { // // NMEA Configuration // - if (getRosBoolean(nh.get(), "nmea/set")) { + if (getRosBoolean(node_, "nmea/set")) { bool compat, consider; - if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.nmea_version)) { + if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } - if (!getRosUint(nh.get(), "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); } - if (!nh->getParam("nmea/compat", compat)) { + if (!node_->getParam("nmea/compat", compat)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/compat must be set"); } - if (!nh->getParam("nmea/consider", consider)) { + if (!node_->getParam("nmea/consider", consider)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/consider must be set"); } @@ -1065,20 +1064,20 @@ void UbloxFirmware7::getRosParams() { cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - getRosUint(nh.get(), "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(nh.get(), "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); } } @@ -1135,16 +1134,16 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { block.gnss_id = block.GNSS_ID_SBAS; block.res_trk_ch = block.RES_TRK_CH_SBAS; block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = getRosBoolean(nh.get(), "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; + block.flags = getRosBoolean(node_, "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(nh.get(), "gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(node_, "gnss/sbas") ? "enable" : "disable") + " SBAS."); } } - if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1155,17 +1154,17 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT (always does so since fix information is published // from this) gps->subscribe(std::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, nh.get(), std::placeholders::_1), + &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean(nh.get(), "publish/nav/svinfo")) { + if (getRosBoolean(node_, "publish/nav/svinfo")) { gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(nh.get(), "publish/mon/hw")) { + if (getRosBoolean(node_, "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } @@ -1176,19 +1175,19 @@ void UbloxFirmware7::subscribe(std::shared_ptr gps) { // void UbloxFirmware8::getRosParams() { // UPD SOS configuration - clear_bbr_ = getRosBoolean(nh.get(), "clear_bbr"); - save_on_shutdown_ = getRosBoolean(nh.get(), "save_on_shutdown"); + clear_bbr_ = getRosBoolean(node_, "clear_bbr"); + save_on_shutdown_ = getRosBoolean(node_, "save_on_shutdown"); // GNSS enable/disable - enable_gps_ = getRosBoolean(nh.get(), "gnss/gps"); - enable_galileo_ = getRosBoolean(nh.get(), "gnss/galileo"); - enable_beidou_ = getRosBoolean(nh.get(), "gnss/beidou"); - enable_imes_ = getRosBoolean(nh.get(), "gnss/imes"); - enable_glonass_ = getRosBoolean(nh.get(), "gnss/glonass"); - enable_qzss_ = getRosBoolean(nh.get(), "gnss/qzss"); + enable_gps_ = getRosBoolean(node_, "gnss/gps"); + enable_galileo_ = getRosBoolean(node_, "gnss/galileo"); + enable_beidou_ = getRosBoolean(node_, "gnss/beidou"); + enable_imes_ = getRosBoolean(node_, "gnss/imes"); + enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); // QZSS Signal Configuration - getRosUint(nh.get(), "gnss/qzss_sig_cfg", qzss_sig_cfg_, + getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { @@ -1214,7 +1213,7 @@ void UbloxFirmware8::getRosParams() { if (enable_qzss_ && !gnss_->isSupported("QZSS")) { ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean(nh.get(), "gnss/sbas") && !gnss_->isSupported("SBAS")) { + if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); } @@ -1227,28 +1226,28 @@ void UbloxFirmware8::getRosParams() { // // NMEA Configuration // - if (getRosBoolean(nh.get(), "nmea/set")) { + if (getRosBoolean(node_, "nmea/set")) { bool compat, consider; cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set - if (!getRosUint(nh.get(), "nmea/version", cfg_nmea_.nmea_version)) { + if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/version must be set"); } - if (!getRosUint(nh.get(), "nmea/num_sv", cfg_nmea_.num_sv)) { + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/num_sv must be set"); } - if (!getRosUint(nh.get(), "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/sv_numbering must be set"); } - if (!nh->getParam("nmea/compat", compat)) { + if (!node_->getParam("nmea/compat", compat)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/compat must be set"); } - if (!nh->getParam("nmea/consider", consider)) { + if (!node_->getParam("nmea/consider", consider)) { throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + "true, therefore nmea/consider must be set"); } @@ -1256,27 +1255,27 @@ void UbloxFirmware8::getRosParams() { // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; - cfg_nmea_.flags |= getRosBoolean(nh.get(), "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(nh.get(), "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(nh.get(), "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - getRosUint(nh.get(), "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(nh.get(), "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); std::vector bds_talker_id; - getRosUint(nh.get(), "nmea/bds_talker_id", bds_talker_id); + getRosUint(node_, "nmea/bds_talker_id", bds_talker_id); cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; } @@ -1316,10 +1315,10 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; ROS_DEBUG("GPS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_SBAS - && getRosBoolean(nh.get(), "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { + && getRosBoolean(node_, "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(nh.get(), "gnss/sbas"); + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss/sbas"); ROS_DEBUG("SBAS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { @@ -1375,7 +1374,7 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // // NMEA config // - if (getRosBoolean(nh.get(), "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -1385,22 +1384,22 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT gps->subscribe( - std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, nh.get(), std::placeholders::_1), 1); + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SAT messages - if (getRosBoolean(nh.get(), "publish/nav/sat")) { + if (getRosBoolean(node_, "publish/nav/sat")) { gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(nh.get(), "publish/mon/hw")) { + if (getRosBoolean(node_, "publish/mon/hw")) { gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, 1); } // Subscribe to RTCM messages - if (getRosBoolean(nh.get(), "publish/rxm/rtcm")) { + if (getRosBoolean(node_, "publish/rxm/rtcm")) { gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, 1); } @@ -1414,54 +1413,54 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater) - : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater) { - rxm_raw_pub_ = nh->advertise("rxmraw", 1); - rxm_sfrb_pub_ = nh->advertise("rxmsfrb", 1); - rxm_eph_pub_ = nh->advertise("rxmeph", 1); - rxm_alm_pub_ = nh->advertise("rxmalm", 1); +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { + rxm_raw_pub_ = node_->advertise("rxmraw", 1); + rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); + rxm_eph_pub_ = node_->advertise("rxmeph", 1); + rxm_alm_pub_ = node_->advertise("rxmalm", 1); } void RawDataProduct::subscribe(std::shared_ptr gps) { // Subscribe to RXM Raw - if (getRosBoolean(nh.get(), "publish/rxm/raw")) { + if (getRosBoolean(node_, "publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, 1); } // Subscribe to RXM SFRB - if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { + if (getRosBoolean(node_, "publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RXM EPH - if (getRosBoolean(nh.get(), "publish/rxm/eph")) { + if (getRosBoolean(node_, "publish/rxm/eph")) { gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, 1); } // Subscribe to RXM ALM - if (getRosBoolean(nh.get(), "publish/rxm/almRaw")) { + if (getRosBoolean(node_, "publish/rxm/almRaw")) { gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, 1); } } void RawDataProduct::initializeRosDiagnostics() { - if (getRosBoolean(nh.get(), "publish/rxm/raw")) { + if (getRosBoolean(node_, "publish/rxm/raw")) { freq_diagnostics_.push_back(std::make_shared( "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { + if (getRosBoolean(node_, "publish/rxm/sfrb")) { freq_diagnostics_.push_back(std::make_shared( "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(nh.get(), "publish/rxm/eph")) { + if (getRosBoolean(node_, "publish/rxm/eph")) { freq_diagnostics_.push_back(std::make_shared( "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(nh.get(), "publish/rxm/almRaw")) { + if (getRosBoolean(node_, "publish/rxm/almRaw")) { freq_diagnostics_.push_back(std::make_shared( "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } @@ -1470,23 +1469,23 @@ void RawDataProduct::initializeRosDiagnostics() { // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater) - : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { imu_pub_ = - nh->advertise("imu_meas", 1); + node_->advertise("imu_meas", 1); time_ref_pub_ = - nh->advertise("interrupt_time", 1); - nav_att_pub_ = nh->advertise("navatt", 1); - esf_ins_pub_ = nh->advertise("esfins", 1); - esf_meas_pub_ = nh->advertise("esfmeas", 1); - esf_raw_pub_ = nh->advertise("esfraw", 1); - esf_status_pub_ = nh->advertise("esfstatus", 1); - hnr_pvt_pub_ = nh->advertise("hnrpvt", 1); + node_->advertise("interrupt_time", 1); + nav_att_pub_ = node_->advertise("navatt", 1); + esf_ins_pub_ = node_->advertise("esfins", 1); + esf_meas_pub_ = node_->advertise("esfmeas", 1); + esf_raw_pub_ = node_->advertise("esfraw", 1); + esf_status_pub_ = node_->advertise("esfstatus", 1); + hnr_pvt_pub_ = node_->advertise("hnrpvt", 1); } void AdrUdrProduct::getRosParams() { - use_adr_ = getRosBoolean(nh.get(), "use_adr"); + use_adr_ = getRosBoolean(node_, "use_adr"); // Check the nav rate float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { @@ -1504,19 +1503,19 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to NAV ATT messages - if (getRosBoolean(nh.get(), "publish/nav/att")) { + if (getRosBoolean(node_, "publish/nav/att")) { gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, 1); } // Subscribe to ESF INS messages - if (getRosBoolean(nh.get(), "publish/esf/ins")) { + if (getRosBoolean(node_, "publish/esf/ins")) { gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, 1); } // Subscribe to ESF Meas messages - if (getRosBoolean(nh.get(), "publish/esf/meas")) { + if (getRosBoolean(node_, "publish/esf/meas")) { gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, 1); // also publish sensor_msgs::Imu @@ -1525,26 +1524,26 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { } // Subscribe to ESF Raw messages - if (getRosBoolean(nh.get(), "publish/esf/raw")) { + if (getRosBoolean(node_, "publish/esf/raw")) { gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, 1); } // Subscribe to ESF Status messages - if (getRosBoolean(nh.get(), "publish/esf/status")) { + if (getRosBoolean(node_, "publish/esf/status")) { gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, 1); } // Subscribe to HNR PVT messages - if (getRosBoolean(nh.get(), "publish/hnr/pvt")) { + if (getRosBoolean(node_, "publish/hnr/pvt")) { gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, 1); } } void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (getRosBoolean(nh.get(), "publish/esf/meas")) { + if (getRosBoolean(node_, "publish/esf/meas")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -1553,7 +1552,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { float deg_c = 1e-2; std::vector imu_data = m.data; - for (int i=0; i < imu_data.size(); i++){ + for (int i = 0; i < imu_data.size(); i++){ unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits @@ -1636,6 +1635,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { updater_->force_update(); } + // // u-blox High Precision GNSS Reference Station // From 4bcb3a3fa000bf16b48867594345b5ee0a751cfc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:17:21 +0000 Subject: [PATCH 064/152] Remove the last uses of the global variable. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 12 +++-- ublox_gps/src/node.cpp | 72 ++++++++++++++-------------- 2 files changed, 44 insertions(+), 40 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 58d336c5..756a70de 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -1022,7 +1022,7 @@ class HpgRefProduct: public virtual ComponentInterface { //! Default measurement period for HPG devices constexpr static uint16_t kDefaultMeasPeriod = 250; - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); /** * @brief Get the ROS parameters specific to the Reference Station @@ -1135,6 +1135,7 @@ class HpgRefProduct: public virtual ComponentInterface { std::vector rtcms_; std::shared_ptr gps_; + ros::NodeHandle* node_; }; /** @@ -1152,7 +1153,7 @@ class HpgRovProduct final : public virtual ComponentInterface { //! Diagnostic updater: RTCM topic frequency window [num messages] constexpr static int kRtcmFreqWindow = 25; - explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater); + explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node); /** * @brief Get the ROS parameters specific to the Rover configuration. @@ -1210,11 +1211,12 @@ class HpgRovProduct final : public virtual ComponentInterface { uint16_t nav_rate_; std::shared_ptr updater_; + ros::NodeHandle* node_; }; class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. @@ -1247,7 +1249,7 @@ class HpPosRecProduct final : public virtual HpgRefProduct { */ class TimProduct final : public virtual ComponentInterface { public: - explicit TimProduct(const std::string & frame_id, std::shared_ptr updater); + explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); /** * @brief Get the Time Sync parameters. @@ -1290,6 +1292,8 @@ class TimProduct final : public virtual ComponentInterface { std::string frame_id_; std::shared_ptr updater_; + + ros::NodeHandle* node_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f3f11483..f3907b94 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -183,13 +183,13 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, nh.get())); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { - components_.push_back(std::make_shared(nav_rate_, updater_)); + components_.push_back(std::make_shared(nav_rate_, updater_, nh.get())); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, nh.get())); } else if (product_category.compare("TIM") == 0) { - components_.push_back(std::make_shared(frame_id_, updater_)); + components_.push_back(std::make_shared(frame_id_, updater_, nh.get())); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, nh.get())); @@ -1640,48 +1640,48 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms) - : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) { navsvin_pub_ = - nh->advertise("navsvin", 1); + node_->advertise("navsvin", 1); } void HpgRefProduct::getRosParams() { - if (getRosBoolean(nh.get(), "config_on_startup")) { + if (getRosBoolean(node_, "config_on_startup")) { if (nav_rate_ * meas_rate_ != 1000) { ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); } - if (!getRosUint(nh.get(), "tmode3", tmode3_)) { + if (!getRosUint(node_, "tmode3", tmode3_)) { throw std::runtime_error("Invalid settings: TMODE3 must be set"); } if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if (!nh->getParam("arp/position", arp_position_)) { + if (!node_->getParam("arp/position", arp_position_)) { throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); } - if (!getRosInt(nh.get(), "arp/position_hp", arp_position_hp_)) { + if (!getRosInt(node_, "arp/position_hp", arp_position_hp_)) { throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); } - if (!nh->getParam("arp/acc", fixed_pos_acc_)) { + if (!node_->getParam("arp/acc", fixed_pos_acc_)) { throw std::runtime_error(std::string("Invalid settings: arp/acc ") + "must be set if TMODE3 is fixed"); } - if (!nh->getParam("arp/lla_flag", lla_flag_)) { + if (!node_->getParam("arp/lla_flag", lla_flag_)) { ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - svin_reset_ = getRosBoolean(nh.get(), "sv_in/reset"); - if (!getRosUint(nh.get(), "sv_in/min_dur", sv_in_min_dur_)) { + svin_reset_ = getRosBoolean(node_, "sv_in/reset"); + if (!getRosUint(node_, "sv_in/min_dur", sv_in_min_dur_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + "must be set if TMODE3 is survey-in"); } - if (!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { + if (!node_->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + "must be set if TMODE3 is survey-in"); } @@ -1773,7 +1773,7 @@ void HpgRefProduct::subscribe(std::shared_ptr gps) { } void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { - if (getRosBoolean(nh.get(), "publish/nav/svin")) { + if (getRosBoolean(node_, "publish/nav/svin")) { navsvin_pub_.publish(m); } @@ -1854,16 +1854,16 @@ void HpgRefProduct::tmode3Diagnostics( // // U-Blox High Precision GNSS Rover // -HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater) - : nav_rate_(nav_rate), updater_(updater) +HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), updater_(updater), node_(node) { nav_rel_pos_ned_pub_ = - nh->advertise("navrelposned", 1); + node_->advertise("navrelposned", 1); } void HpgRovProduct::getRosParams() { // default to float, see CfgDGNSS message for details - getRosUint(nh.get(), "dgnss_mode", dgnss_mode_, + getRosUint(node_, "dgnss_mode", dgnss_mode_, ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } @@ -1925,7 +1925,7 @@ void HpgRovProduct::carrierPhaseDiagnostics( } void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (getRosBoolean(nh.get(), "publish/nav/relposned")) { + if (getRosBoolean(node_, "publish/nav/relposned")) { nav_rel_pos_ned_pub_.publish(m); } @@ -1936,14 +1936,14 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms) - : HpgRefProduct(nav_rate, meas_rate, updater, rtcms), frame_id_(frame_id) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) + : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) { nav_relposned_pub_ = - nh->advertise("navrelposned", 1); + node_->advertise("navrelposned", 1); imu_pub_ = - nh->advertise("navheading", 1); + node_->advertise("navheading", 1); } void HpPosRecProduct::subscribe(std::shared_ptr gps) { @@ -1954,11 +1954,11 @@ void HpPosRecProduct::subscribe(std::shared_ptr gps) { } void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (getRosBoolean(nh.get(), "publish/nav/relposned")) { + if (getRosBoolean(node_, "publish/nav/relposned")) { nav_relposned_pub_.publish(m); } - if (getRosBoolean(nh.get(), "publish/nav/heading")) { + if (getRosBoolean(node_, "publish/nav/heading")) { imu_.header.stamp = ros::Time::now(); imu_.header.frame_id = frame_id_; @@ -1987,14 +1987,14 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // // U-Blox Time Sync Products, partially implemented. // -TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater) : frame_id_(frame_id), updater_(updater) +TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) : frame_id_(frame_id), updater_(updater), node_(node) { timtm2_pub_ = - nh->advertise("timtm2", 1); + node_->advertise("timtm2", 1); interrupt_time_pub_ = - nh->advertise("interrupt_time", 1); - rxm_sfrb_pub_ = nh->advertise("rxmsfrb", 1); - rxm_raw_pub_ = nh->advertise("rxmraw", 1); + node_->advertise("interrupt_time", 1); + rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); + rxm_raw_pub_ = node_->advertise("rxmraw", 1); } void TimProduct::getRosParams() { @@ -2021,13 +2021,13 @@ void TimProduct::subscribe(std::shared_ptr gps) { ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages - if (getRosBoolean(nh.get(), "publish/rxm/sfrb")) { + if (getRosBoolean(node_, "publish/rxm/sfrb")) { gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, 1); } // Subscribe to RawX messages - if (getRosBoolean(nh.get(), "publish/rxm/raw")) { + if (getRosBoolean(node_, "publish/rxm/raw")) { gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, 1); } @@ -2035,7 +2035,7 @@ void TimProduct::subscribe(std::shared_ptr gps) { void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - if (getRosBoolean(nh.get(), "publish/tim/tm2")) { + if (getRosBoolean(node_, "publish/tim/tm2")) { // create time ref message and put in the data t_ref_.header.seq = m.rising_edge_count; t_ref_.header.stamp = ros::Time::now(); From e22311b902163eca189acc36c141f97711bc0baa Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:19:54 +0000 Subject: [PATCH 065/152] Make the node handle a member variable of UbloxNode. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 8 +- ublox_gps/src/node.cpp | 324 +++++++++++++-------------- 2 files changed, 166 insertions(+), 166 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 756a70de..29c93808 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -82,10 +82,6 @@ */ namespace ublox_node { -// ROS objects -//! Node Handle for GPS node -std::shared_ptr nh; - /** * @brief Check that the parameter is above the minimum. * @param val the value to check @@ -484,6 +480,10 @@ class UbloxNode final { //! Handles communication with the U-Blox Device std::shared_ptr gps_; + + //! Node Handle for GPS node + std::shared_ptr nh_; + }; /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f3907b94..1a7bffa0 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -137,18 +137,27 @@ std::vector stringSplit(const std::string &str, // u-blox ROS Node // UbloxNode::UbloxNode() { + nh_ = std::make_shared("~"); + int debug; - nh->param("debug", debug, 1); + nh_->param("debug", debug, 1); + if (debug) { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } + } + gps_ = std::make_shared(debug); gnss_ = std::make_shared(); - nav_status_pub_ = nh->advertise("navstatus", 1); - nav_posecef_pub_ = nh->advertise("navposecef", 1); - nav_clock_pub_ = nh->advertise("navclock", 1); - aid_alm_pub_ = nh->advertise("aidalm", 1); - aid_eph_pub_ = nh->advertise("aideph", 1); - aid_hui_pub_ = nh->advertise("aidhui", 1); + nav_status_pub_ = nh_->advertise("navstatus", 1); + nav_posecef_pub_ = nh_->advertise("navposecef", 1); + nav_clock_pub_ = nh_->advertise("navclock", 1); + aid_alm_pub_ = nh_->advertise("aidalm", 1); + aid_eph_pub_ = nh_->advertise("aideph", 1); + aid_hui_pub_ = nh_->advertise("aidhui", 1); updater_ = std::make_shared(); updater_->setHardwareID("ublox"); @@ -163,16 +172,16 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); ublox_version = 9; } @@ -183,16 +192,16 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, nh.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, nh_.get())); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { - components_.push_back(std::make_shared(nav_rate_, updater_, nh.get())); + components_.push_back(std::make_shared(nav_rate_, updater_, nh_.get())); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, nh.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, nh_.get())); } else if (product_category.compare("TIM") == 0) { - components_.push_back(std::make_shared(frame_id_, updater_, nh.get())); + components_.push_back(std::make_shared(frame_id_, updater_, nh_.get())); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, nh.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, nh_.get())); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { @@ -203,60 +212,60 @@ void UbloxNode::addProductInterface(const std::string & product_category, } void UbloxNode::getRosParams() { - nh->param("device", device_, std::string("/dev/ttyACM0")); - nh->param("frame_id", frame_id_, std::string("gps")); + nh_->param("device", device_, std::string("/dev/ttyACM0")); + nh_->param("frame_id", frame_id_, std::string("gps")); // Save configuration parameters - getRosUint(nh.get(), "load/mask", load_.load_mask, 0); - getRosUint(nh.get(), "load/device", load_.device_mask, 0); - getRosUint(nh.get(), "save/mask", save_.save_mask, 0); - getRosUint(nh.get(), "save/device", save_.device_mask, 0); + getRosUint(nh_.get(), "load/mask", load_.load_mask, 0); + getRosUint(nh_.get(), "load/device", load_.device_mask, 0); + getRosUint(nh_.get(), "save/mask", save_.save_mask, 0); + getRosUint(nh_.get(), "save/device", save_.device_mask, 0); // UART 1 params - getRosUint(nh.get(), "uart1/baudrate", baudrate_, 9600); - getRosUint(nh.get(), "uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX + getRosUint(nh_.get(), "uart1/baudrate", baudrate_, 9600); + getRosUint(nh_.get(), "uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX | ublox_msgs::CfgPRT::PROTO_NMEA | ublox_msgs::CfgPRT::PROTO_RTCM); - getRosUint(nh.get(), "uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); + getRosUint(nh_.get(), "uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) { + if (nh_->hasParam("usb/in") || nh_->hasParam("usb/out")) { set_usb_ = true; - if (!getRosUint(nh.get(), "usb/in", usb_in_)) { + if (!getRosUint(nh_.get(), "usb/in", usb_in_)) { throw std::runtime_error(std::string("usb/out is set, therefore ") + "usb/in must be set"); } - if (!getRosUint(nh.get(), "usb/out", usb_out_)) { + if (!getRosUint(nh_.get(), "usb/out", usb_out_)) { throw std::runtime_error(std::string("usb/in is set, therefore ") + "usb/out must be set"); } - getRosUint(nh.get(), "usb/tx_ready", usb_tx_, 0); + getRosUint(nh_.get(), "usb/tx_ready", usb_tx_, 0); } // Measurement rate params - nh->param("rate", rate_, 4.0); // in Hz - getRosUint(nh.get(), "nav_rate", nav_rate_, 1); // # of measurement rate cycles + nh_->param("rate", rate_, 4.0); // in Hz + getRosUint(nh_.get(), "nav_rate", nav_rate_, 1); // # of measurement rate cycles // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; - getRosUint(nh.get(), "rtcm/ids", rtcm_ids); // RTCM output message IDs - getRosUint(nh.get(), "rtcm/rates", rtcm_rates); // RTCM output message rates + getRosUint(nh_.get(), "rtcm/ids", rtcm_ids); // RTCM output message IDs + getRosUint(nh_.get(), "rtcm/rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting - declareRosBoolean(nh.get(), "enable_ppp", false); + declareRosBoolean(nh_.get(), "enable_ppp", false); // SBAS params, only for some devices - declareRosBoolean(nh.get(), "gnss/sbas", false); - declareRosBoolean(nh.get(), "gnss/gps", true); - declareRosBoolean(nh.get(), "gnss/glonass", false); - declareRosBoolean(nh.get(), "gnss/qzss", false); - declareRosBoolean(nh.get(), "gnss/galileo", false); - declareRosBoolean(nh.get(), "gnss/beidou", false); - declareRosBoolean(nh.get(), "gnss/imes", false); - getRosUint(nh.get(), "sbas/max", max_sbas_, 0); // Maximum number of SBAS channels - getRosUint(nh.get(), "sbas/usage", sbas_usage_, 0); - nh->param("dynamic_model", dynamic_model_, std::string("portable")); - nh->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint(nh.get(), "dr_limit", dr_limit_, 0); // Dead reckoning limit - - if (getRosBoolean(nh.get(), "enable_ppp")) { + declareRosBoolean(nh_.get(), "gnss/sbas", false); + declareRosBoolean(nh_.get(), "gnss/gps", true); + declareRosBoolean(nh_.get(), "gnss/glonass", false); + declareRosBoolean(nh_.get(), "gnss/qzss", false); + declareRosBoolean(nh_.get(), "gnss/galileo", false); + declareRosBoolean(nh_.get(), "gnss/beidou", false); + declareRosBoolean(nh_.get(), "gnss/imes", false); + getRosUint(nh_.get(), "sbas/max", max_sbas_, 0); // Maximum number of SBAS channels + getRosUint(nh_.get(), "sbas/usage", sbas_usage_, 0); + nh_->param("dynamic_model", dynamic_model_, std::string("portable")); + nh_->param("fix_mode", fix_mode_, std::string("auto")); + getRosUint(nh_.get(), "dr_limit", dr_limit_, 0); // Dead reckoning limit + + if (getRosBoolean(nh_.get(), "enable_ppp")) { ROS_WARN("Warning: PPP is enabled - this is an expert setting."); } @@ -276,14 +285,14 @@ void UbloxNode::getRosParams() { dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); - declareRosBoolean(nh.get(), "dat/set", false); - if (getRosBoolean(nh.get(), "dat/set")) { + declareRosBoolean(nh_.get(), "dat/set", false); + if (getRosBoolean(nh_.get(), "dat/set")) { std::vector shift, rot; - if (!nh->getParam("dat/majA", cfg_dat_.maj_a) - || nh->getParam("dat/flat", cfg_dat_.flat) - || nh->getParam("dat/shift", shift) - || nh->getParam("dat/rot", rot) - || nh->getParam("dat/scale", cfg_dat_.scale)) { + if (!nh_->getParam("dat/majA", cfg_dat_.maj_a) + || nh_->getParam("dat/flat", cfg_dat_.flat) + || nh_->getParam("dat/shift", shift) + || nh_->getParam("dat/rot", rot) + || nh_->getParam("dat/scale", cfg_dat_.scale)) { throw std::runtime_error(std::string("dat/set is true, therefore ") + "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); } @@ -311,99 +320,99 @@ void UbloxNode::getRosParams() { meas_rate_ = 1000 / rate_; // activate/deactivate any config - declareRosBoolean(nh.get(), "config_on_startup", true); - declareRosBoolean(nh.get(), "raw_data", false); - declareRosBoolean(nh.get(), "clear_bbr", false); - declareRosBoolean(nh.get(), "save_on_shutdown", false); - declareRosBoolean(nh.get(), "use_adr", true); + declareRosBoolean(nh_.get(), "config_on_startup", true); + declareRosBoolean(nh_.get(), "raw_data", false); + declareRosBoolean(nh_.get(), "clear_bbr", false); + declareRosBoolean(nh_.get(), "save_on_shutdown", false); + declareRosBoolean(nh_.get(), "use_adr", true); - declareRosBoolean(nh.get(), "sv_in/reset", true); + declareRosBoolean(nh_.get(), "sv_in/reset", true); // raw data stream logging rawDataStreamPa_.getRosParams(); // NMEA parameters - declareRosBoolean(nh.get(), "nmea/set", false); - declareRosBoolean(nh.get(), "nmea/compat", false); - declareRosBoolean(nh.get(), "nmea/consider", false); - declareRosBoolean(nh.get(), "nmea/limit82", false); - declareRosBoolean(nh.get(), "nmea/high_prec", false); - declareRosBoolean(nh.get(), "nmea/filter/pos", false); - declareRosBoolean(nh.get(), "nmea/filter/msk_pos", false); - declareRosBoolean(nh.get(), "nmea/filter/time", false); - declareRosBoolean(nh.get(), "nmea/filter/date", false); - declareRosBoolean(nh.get(), "nmea/filter/sbas", false); - declareRosBoolean(nh.get(), "nmea/filter/track", false); - declareRosBoolean(nh.get(), "nmea/filter/gps_only", false); - declareRosBoolean(nh.get(), "nmea/gnssToFilter/gps", false); - declareRosBoolean(nh.get(), "nmea/gnssToFilter/sbas", false); - declareRosBoolean(nh.get(), "nmea/gnssToFilter/qzss", false); - declareRosBoolean(nh.get(), "nmea/gnssToFilter/glonass", false); - declareRosBoolean(nh.get(), "nmea/gnssToFilter/beidou", false); + declareRosBoolean(nh_.get(), "nmea/set", false); + declareRosBoolean(nh_.get(), "nmea/compat", false); + declareRosBoolean(nh_.get(), "nmea/consider", false); + declareRosBoolean(nh_.get(), "nmea/limit82", false); + declareRosBoolean(nh_.get(), "nmea/high_prec", false); + declareRosBoolean(nh_.get(), "nmea/filter/pos", false); + declareRosBoolean(nh_.get(), "nmea/filter/msk_pos", false); + declareRosBoolean(nh_.get(), "nmea/filter/time", false); + declareRosBoolean(nh_.get(), "nmea/filter/date", false); + declareRosBoolean(nh_.get(), "nmea/filter/sbas", false); + declareRosBoolean(nh_.get(), "nmea/filter/track", false); + declareRosBoolean(nh_.get(), "nmea/filter/gps_only", false); + declareRosBoolean(nh_.get(), "nmea/gnssToFilter/gps", false); + declareRosBoolean(nh_.get(), "nmea/gnssToFilter/sbas", false); + declareRosBoolean(nh_.get(), "nmea/gnssToFilter/qzss", false); + declareRosBoolean(nh_.get(), "nmea/gnssToFilter/glonass", false); + declareRosBoolean(nh_.get(), "nmea/gnssToFilter/beidou", false); // Publish parameters - declareRosBoolean(nh.get(), "publish/all", false); - - declareRosBoolean(nh.get(), "publish/nav/all", getRosBoolean(nh.get(), "publish/all")); - declareRosBoolean(nh.get(), "publish/nav/att", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/clock", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/heading", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/posecef", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/posllh", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/pvt", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/relposned", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/sat", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/sol", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/svin", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/svinfo", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/status", getRosBoolean(nh.get(), "publish/nav/all")); - declareRosBoolean(nh.get(), "publish/nav/velned", getRosBoolean(nh.get(), "publish/nav/all")); - - declareRosBoolean(nh.get(), "publish/rxm/all", getRosBoolean(nh.get(), "publish/all")); - declareRosBoolean(nh.get(), "publish/rxm/almRaw", getRosBoolean(nh.get(), "publish/rxm/all")); - declareRosBoolean(nh.get(), "publish/rxm/eph", getRosBoolean(nh.get(), "publish/rxm/all")); - declareRosBoolean(nh.get(), "publish/rxm/rtcm", getRosBoolean(nh.get(), "publish/rxm/all")); - declareRosBoolean(nh.get(), "publish/rxm/raw", getRosBoolean(nh.get(), "publish/rxm/all")); - declareRosBoolean(nh.get(), "publish/rxm/sfrb", getRosBoolean(nh.get(), "publish/rxm/all")); - - declareRosBoolean(nh.get(), "publish/aid/all", getRosBoolean(nh.get(), "publish/all")); - declareRosBoolean(nh.get(), "publish/aid/alm", getRosBoolean(nh.get(), "publish/aid/all")); - declareRosBoolean(nh.get(), "publish/aid/eph", getRosBoolean(nh.get(), "publish/aid/all")); - declareRosBoolean(nh.get(), "publish/aid/hui", getRosBoolean(nh.get(), "publish/aid/all")); - - declareRosBoolean(nh.get(), "publish/mon/all", getRosBoolean(nh.get(), "publish/all")); - declareRosBoolean(nh.get(), "publish/mon/hw", getRosBoolean(nh.get(), "publish/mon/all")); - - declareRosBoolean(nh.get(), "publish/tim/tm2", false); + declareRosBoolean(nh_.get(), "publish/all", false); + + declareRosBoolean(nh_.get(), "publish/nav/all", getRosBoolean(nh_.get(), "publish/all")); + declareRosBoolean(nh_.get(), "publish/nav/att", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/clock", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/heading", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/posecef", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/posllh", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/pvt", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/relposned", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/sat", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/sol", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/svin", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/svinfo", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/status", getRosBoolean(nh_.get(), "publish/nav/all")); + declareRosBoolean(nh_.get(), "publish/nav/velned", getRosBoolean(nh_.get(), "publish/nav/all")); + + declareRosBoolean(nh_.get(), "publish/rxm/all", getRosBoolean(nh_.get(), "publish/all")); + declareRosBoolean(nh_.get(), "publish/rxm/almRaw", getRosBoolean(nh_.get(), "publish/rxm/all")); + declareRosBoolean(nh_.get(), "publish/rxm/eph", getRosBoolean(nh_.get(), "publish/rxm/all")); + declareRosBoolean(nh_.get(), "publish/rxm/rtcm", getRosBoolean(nh_.get(), "publish/rxm/all")); + declareRosBoolean(nh_.get(), "publish/rxm/raw", getRosBoolean(nh_.get(), "publish/rxm/all")); + declareRosBoolean(nh_.get(), "publish/rxm/sfrb", getRosBoolean(nh_.get(), "publish/rxm/all")); + + declareRosBoolean(nh_.get(), "publish/aid/all", getRosBoolean(nh_.get(), "publish/all")); + declareRosBoolean(nh_.get(), "publish/aid/alm", getRosBoolean(nh_.get(), "publish/aid/all")); + declareRosBoolean(nh_.get(), "publish/aid/eph", getRosBoolean(nh_.get(), "publish/aid/all")); + declareRosBoolean(nh_.get(), "publish/aid/hui", getRosBoolean(nh_.get(), "publish/aid/all")); + + declareRosBoolean(nh_.get(), "publish/mon/all", getRosBoolean(nh_.get(), "publish/all")); + declareRosBoolean(nh_.get(), "publish/mon/hw", getRosBoolean(nh_.get(), "publish/mon/all")); + + declareRosBoolean(nh_.get(), "publish/tim/tm2", false); // INF parameters - declareRosBoolean(nh.get(), "inf/all", true); - declareRosBoolean(nh.get(), "inf/debug", false); - declareRosBoolean(nh.get(), "inf/error", getRosBoolean(nh.get(), "inf/all")); - declareRosBoolean(nh.get(), "inf/notice", getRosBoolean(nh.get(), "inf/all")); - declareRosBoolean(nh.get(), "inf/test", getRosBoolean(nh.get(), "inf/all")); - declareRosBoolean(nh.get(), "inf/warning", getRosBoolean(nh.get(), "inf/all")); + declareRosBoolean(nh_.get(), "inf/all", true); + declareRosBoolean(nh_.get(), "inf/debug", false); + declareRosBoolean(nh_.get(), "inf/error", getRosBoolean(nh_.get(), "inf/all")); + declareRosBoolean(nh_.get(), "inf/notice", getRosBoolean(nh_.get(), "inf/all")); + declareRosBoolean(nh_.get(), "inf/test", getRosBoolean(nh_.get(), "inf/all")); + declareRosBoolean(nh_.get(), "inf/warning", getRosBoolean(nh_.get(), "inf/all")); // ESF parameters - declareRosBoolean(nh.get(), "publish/esf/all", true); - declareRosBoolean(nh.get(), "publish/esf/ins", getRosBoolean(nh.get(), "publish/esf/all")); - declareRosBoolean(nh.get(), "publish/esf/meas", getRosBoolean(nh.get(), "publish/esf/all")); - declareRosBoolean(nh.get(), "publish/esf/raw", getRosBoolean(nh.get(), "publish/esf/all")); - declareRosBoolean(nh.get(), "publish/esf/status", getRosBoolean(nh.get(), "publish/esf/all")); + declareRosBoolean(nh_.get(), "publish/esf/all", true); + declareRosBoolean(nh_.get(), "publish/esf/ins", getRosBoolean(nh_.get(), "publish/esf/all")); + declareRosBoolean(nh_.get(), "publish/esf/meas", getRosBoolean(nh_.get(), "publish/esf/all")); + declareRosBoolean(nh_.get(), "publish/esf/raw", getRosBoolean(nh_.get(), "publish/esf/all")); + declareRosBoolean(nh_.get(), "publish/esf/status", getRosBoolean(nh_.get(), "publish/esf/all")); // HNR parameters - declareRosBoolean(nh.get(), "publish/hnr/pvt", true); + declareRosBoolean(nh_.get(), "publish/hnr/pvt", true); } void UbloxNode::pollMessages(const ros::TimerEvent& event) { static std::vector payload(1, 1); - if (getRosBoolean(nh.get(), "publish/aid/alm")) { + if (getRosBoolean(nh_.get(), "publish/aid/alm")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } - if (getRosBoolean(nh.get(), "publish/aid/eph")) { + if (getRosBoolean(nh_.get(), "publish/aid/eph")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } - if (getRosBoolean(nh.get(), "publish/aid/hui")) { + if (getRosBoolean(nh_.get(), "publish/aid/hui")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } @@ -430,51 +439,51 @@ void UbloxNode::subscribe() { // subscribe messages // Nav Messages - if (getRosBoolean(nh.get(), "publish/nav/status")) { + if (getRosBoolean(nh_.get(), "publish/nav/status")) { gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, 1); } - if (getRosBoolean(nh.get(), "publish/nav/posecef")) { + if (getRosBoolean(nh_.get(), "publish/nav/posecef")) { gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, 1); } - if (getRosBoolean(nh.get(), "publish/nav/clock")) { + if (getRosBoolean(nh_.get(), "publish/nav/clock")) { gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, 1); } // INF messages - if (getRosBoolean(nh.get(), "inf/debug")) { + if (getRosBoolean(nh_.get(), "inf/debug")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); } - if (getRosBoolean(nh.get(), "inf/error")) { + if (getRosBoolean(nh_.get(), "inf/error")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); } - if (getRosBoolean(nh.get(), "inf/notice")) { + if (getRosBoolean(nh_.get(), "inf/notice")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); } - if (getRosBoolean(nh.get(), "inf/test")) { + if (getRosBoolean(nh_.get(), "inf/test")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); } - if (getRosBoolean(nh.get(), "inf/warning")) { + if (getRosBoolean(nh_.get(), "inf/warning")) { gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), @@ -482,17 +491,17 @@ void UbloxNode::subscribe() { } // AID messages - if (getRosBoolean(nh.get(), "publish/aid/alm")) { + if (getRosBoolean(nh_.get(), "publish/aid/alm")) { gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, 1); } - if (getRosBoolean(nh.get(), "publish/aid/eph")) { + if (getRosBoolean(nh_.get(), "publish/aid/eph")) { gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, 1); } - if (getRosBoolean(nh.get(), "publish/aid/hui")) { + if (getRosBoolean(nh_.get(), "publish/aid/hui")) { gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, 1); } @@ -503,7 +512,7 @@ void UbloxNode::subscribe() { } void UbloxNode::initializeRosDiagnostics() { - declareRosBoolean(nh.get(), "diagnostic_period", kDiagnosticPeriod); + declareRosBoolean(nh_.get(), "diagnostic_period", kDiagnosticPeriod); for (int i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); @@ -603,7 +612,7 @@ bool UbloxNode::configureUblox() { } } - if (getRosBoolean(nh.get(), "config_on_startup")) { + if (getRosBoolean(nh_.get(), "config_on_startup")) { if (set_usb_) { gps_->configUsb(usb_tx_, usb_in_, usb_out_); } @@ -615,15 +624,15 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { - if (!gps_->configSbas(getRosBoolean(nh.get(), "gnss/sbas"), sbas_usage_, max_sbas_)) { + if (!gps_->configSbas(getRosBoolean(nh_.get(), "gnss/sbas"), sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(nh.get(), "gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(nh_.get(), "gnss/sbas") ? "enable" : "disable") + " SBAS."); } } - if (!gps_->setPpp(getRosBoolean(nh.get(), "enable_ppp"))) { + if (!gps_->setPpp(getRosBoolean(nh_.get(), "enable_ppp"))) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(nh.get(), "enable_ppp") ? "enable" : "disable") + (getRosBoolean(nh_.get(), "enable_ppp") ? "enable" : "disable") + " PPP."); } if (!gps_->setDynamicModel(dmodel_)) { @@ -637,7 +646,7 @@ bool UbloxNode::configureUblox() { ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (getRosBoolean(nh.get(), "dat/set") && !gps_->configure(cfg_dat_)) { + if (getRosBoolean(nh_.get(), "dat/set") && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component @@ -667,11 +676,11 @@ void UbloxNode::configureInf() { ublox_msgs::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (getRosBoolean(nh.get(), "inf/error") ? block.INF_MSG_ERROR : 0) | - (getRosBoolean(nh.get(), "inf/warning") ? block.INF_MSG_WARNING : 0) | - (getRosBoolean(nh.get(), "inf/notice") ? block.INF_MSG_NOTICE : 0) | - (getRosBoolean(nh.get(), "inf/test") ? block.INF_MSG_TEST : 0) | - (getRosBoolean(nh.get(), "inf/debug") ? block.INF_MSG_DEBUG : 0); + uint8_t mask = (getRosBoolean(nh_.get(), "inf/error") ? block.INF_MSG_ERROR : 0) | + (getRosBoolean(nh_.get(), "inf/warning") ? block.INF_MSG_WARNING : 0) | + (getRosBoolean(nh_.get(), "inf/notice") ? block.INF_MSG_NOTICE : 0) | + (getRosBoolean(nh_.get(), "inf/test") ? block.INF_MSG_TEST : 0) | + (getRosBoolean(nh_.get(), "inf/debug") ? block.INF_MSG_DEBUG : 0); for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } @@ -696,7 +705,7 @@ void UbloxNode::configureInf() { } void UbloxNode::initializeIo() { - gps_->setConfigOnStartup(getRosBoolean(nh.get(), "config_on_startup")); + gps_->setConfigOnStartup(getRosBoolean(nh_.get(), "config_on_startup")); std::smatch match; if (std::regex_match(device_, match, @@ -730,8 +739,8 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) { - if (getRosBoolean(nh.get(), "raw_data")) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, nh.get())); + if (getRosBoolean(nh_.get(), "raw_data")) { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, nh_.get())); } } // Must set firmware & hardware params before initializing diagnostics @@ -749,7 +758,7 @@ void UbloxNode::initialize() { configureInf(); ros::Timer poller; - poller = nh->createTimer(ros::Duration(kPollDuration), + poller = nh_->createTimer(ros::Duration(kPollDuration), &UbloxNode::pollMessages, this); poller.start(); @@ -2065,15 +2074,6 @@ void TimProduct::initializeRosDiagnostics() { int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); - ublox_node::nh = std::make_shared("~"); - int debug; - ublox_node::nh->param("debug", debug, 1); - if (debug) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } - } ublox_node::UbloxNode node; return 0; } From b3d25845f730e62fea3f8ff9dbf26ed4e51e10fb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:31:25 +0000 Subject: [PATCH 066/152] Move TimProduct class into its own file. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 247 -------------------- ublox_gps/include/ublox_gps/tim_product.hpp | 73 ++++++ ublox_gps/include/ublox_gps/utils.hpp | 210 ++++++++++++++++- ublox_gps/src/node.cpp | 78 +------ ublox_gps/src/tim_product.cpp | 96 ++++++++ 6 files changed, 379 insertions(+), 327 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/tim_product.hpp create mode 100644 ublox_gps/src/tim_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 5dd657e7..48989da8 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 29c93808..178b88ee 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -82,200 +82,6 @@ */ namespace ublox_node { -/** - * @brief Check that the parameter is above the minimum. - * @param val the value to check - * @param min the minimum for this value - * @param name the name of the parameter - * @throws std::runtime_error if it is below the minimum - */ -template -void checkMin(V val, T min, const std::string & name) { - if (val < min) { - std::stringstream oss; - oss << "Invalid settings: " << name << " must be > " << min; - throw std::runtime_error(oss.str()); - } -} - -/** - * @brief Check that the parameter is in the range. - * @param val the value to check - * @param min the minimum for this value - * @param max the maximum for this value - * @param name the name of the parameter - * @throws std::runtime_error if it is out of bounds - */ -template -void checkRange(V val, T min, T max, const std::string & name) { - if (val < min || val > max) { - std::stringstream oss; - oss << "Invalid settings: " << name << " must be in range [" << min << - ", " << max << "]."; - throw std::runtime_error(oss.str()); - } -} - -/** - * @brief Check that the elements of the vector are in the range. - * @param val the vector to check - * @param min the minimum for this value - * @param max the maximum for this value - * @param name the name of the parameter - * @throws std::runtime_error value it is out of bounds - */ -template -void checkRange(std::vector val, T min, T max, const std::string & name) { - for (size_t i = 0; i < val.size(); i++) { - std::stringstream oss; - oss << name << "[" << i << "]"; - checkRange(val[i], min, max, oss.str()); - } -} - -/** - * @brief Get a unsigned integer value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -bool getRosUint(ros::NodeHandle* node, const std::string& key, U &u) { - int param; - if (!node->getParam(key, param)) { - return false; - } - // Check the bounds - U min = std::numeric_limits::lowest(); - U max = std::numeric_limits::max(); - checkRange(param, min, max, key); - // set the output - u = (U) param; - return true; -} - -/** - * @brief Get a unsigned integer value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @param val value to use if the server doesn't contain this parameter. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -void getRosUint(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { - if (!getRosUint(node, key, u)) { - u = default_val; - } -} - -/** - * @brief Get a unsigned integer vector from the parameter server. - * @throws std::runtime_error if the parameter is out of bounds. - * @return true if found, false if not found. - */ -template -bool getRosUint(ros::NodeHandle* node, const std::string& key, std::vector &u) { - std::vector param; - if (!node->getParam(key, param)) { - return false; - } - - // Check the bounds - U min = std::numeric_limits::lowest(); - U max = std::numeric_limits::max(); - checkRange(param, min, max, key); - - // set the output - u.insert(u.begin(), param.begin(), param.end()); - return true; -} - -/** - * @brief Get a integer (size 8 or 16) value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -bool getRosInt(ros::NodeHandle* node, const std::string& key, I &u) { - int param; - if (!node->getParam(key, param)) { - return false; - } - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - // set the output - u = (I) param; - return true; -} - -/** - * @brief Get an integer value (size 8 or 16) from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @param val value to use if the server doesn't contain this parameter. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -void getRosInt(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { - if (!getRosInt(node, key, u)) { - u = default_val; - } -} - -/** - * @brief Get a int (size 8 or 16) vector from the parameter server. - * @throws std::runtime_error if the parameter is out of bounds. - * @return true if found, false if not found. - */ -template -bool getRosInt(ros::NodeHandle* node, const std::string& key, std::vector &i) { - std::vector param; - if (!node->getParam(key, param)) { - return false; - } - - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - - // set the output - i.insert(i.begin(), param.begin(), param.end()); - return true; -} - -bool declareRosBoolean(ros::NodeHandle* node, const std::string &name, bool default_value) -{ - bool ret; - - if (!node->hasParam(name)) { - node->setParam(name, default_value); - } - // implicit else: If the ROS node already has the parameter, just leave it - - if (!node->getParam(name, ret)) { - throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); - } -} - -bool getRosBoolean(ros::NodeHandle* node, const std::string &name) -{ - bool ret; - if (!node->getParam(name, ret)) { - // Note that if this is used after declareRosBoolean, this should never happen. - throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); - } - - return ret; -} - /** * @brief This class represents u-blox ROS node for *all* firmware and product * versions. @@ -1243,59 +1049,6 @@ class HpPosRecProduct final : public virtual HpgRefProduct { std::string frame_id_; }; -/** - * @brief Implements functions for Time Sync products. - * @todo partially implemented - */ -class TimProduct final : public virtual ComponentInterface { - public: - explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); - - /** - * @brief Get the Time Sync parameters. - * @todo Currently unimplemented. - */ - void getRosParams() override; - - /** - * @brief Configure Time Sync settings. - * @todo Currently unimplemented. - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Adds diagnostic updaters for Time Sync status. - * @todo Currently unimplemented. - */ - void initializeRosDiagnostics() override; - - /** - * @brief Subscribe to Time Sync messages. - * - * @details Subscribes to RxmRAWX & RxmSFRBX messages. - */ - void subscribe(std::shared_ptr gps) override; - - private: - /** - * @brief - * @details Publish recieved TimTM2 messages if enabled - */ - void callbackTimTM2(const ublox_msgs::TimTM2 &m); - - sensor_msgs::TimeReference t_ref_; - - ros::Publisher timtm2_pub_; - ros::Publisher interrupt_time_pub_; - ros::Publisher rxm_sfrb_pub_; - ros::Publisher rxm_raw_pub_; - - std::string frame_id_; - std::shared_ptr updater_; - - ros::NodeHandle* node_; -}; - } #endif // UBLOX_GPS_NODE_HPP diff --git a/ublox_gps/include/ublox_gps/tim_product.hpp b/ublox_gps/include/ublox_gps/tim_product.hpp new file mode 100644 index 00000000..5b22defc --- /dev/null +++ b/ublox_gps/include/ublox_gps/tim_product.hpp @@ -0,0 +1,73 @@ +#ifndef UBLOX_GPS_TIM_PRODUCT_HPP +#define UBLOX_GPS_TIM_PRODUCT_HPP + +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Time Sync products. + * @todo partially implemented + */ +class TimProduct final : public virtual ComponentInterface { + public: + explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); + + /** + * @brief Get the Time Sync parameters. + * @todo Currently unimplemented. + */ + void getRosParams() override; + + /** + * @brief Configure Time Sync settings. + * @todo Currently unimplemented. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Adds diagnostic updaters for Time Sync status. + * @todo Currently unimplemented. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Time Sync messages. + * + * @details Subscribes to RxmRAWX & RxmSFRBX messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + /** + * @brief + * @details Publish recieved TimTM2 messages if enabled + */ + void callbackTimTM2(const ublox_msgs::TimTM2 &m); + + sensor_msgs::TimeReference t_ref_; + + ros::Publisher timtm2_pub_; + ros::Publisher interrupt_time_pub_; + ros::Publisher rxm_sfrb_pub_; + ros::Publisher rxm_raw_pub_; + + std::string frame_id_; + std::shared_ptr updater_; + + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_TIM_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index bfec002c..b9314d6c 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -1,9 +1,19 @@ #ifndef UBLOX_GPS_UTILS_HPP #define UBLOX_GPS_UTILS_HPP +#include +#include +#include +#include +#include + #include -#include "ublox_gps/mkgmtime.h" +#include + +#include + +namespace ublox_node { /** * @brief Convert date/time to UTC time in seconds. @@ -24,4 +34,200 @@ long toUtcSeconds(const NavPVT& msg) { return mkgmtime(&time); } -#endif +/** + * @brief Check that the parameter is above the minimum. + * @param val the value to check + * @param min the minimum for this value + * @param name the name of the parameter + * @throws std::runtime_error if it is below the minimum + */ +template +void checkMin(V val, T min, const std::string & name) { + if (val < min) { + std::stringstream oss; + oss << "Invalid settings: " << name << " must be > " << min; + throw std::runtime_error(oss.str()); + } +} + +/** + * @brief Check that the parameter is in the range. + * @param val the value to check + * @param min the minimum for this value + * @param max the maximum for this value + * @param name the name of the parameter + * @throws std::runtime_error if it is out of bounds + */ +template +void checkRange(V val, T min, T max, const std::string & name) { + if (val < min || val > max) { + std::stringstream oss; + oss << "Invalid settings: " << name << " must be in range [" << min << + ", " << max << "]."; + throw std::runtime_error(oss.str()); + } +} + +/** + * @brief Check that the elements of the vector are in the range. + * @param val the vector to check + * @param min the minimum for this value + * @param max the maximum for this value + * @param name the name of the parameter + * @throws std::runtime_error value it is out of bounds + */ +template +void checkRange(std::vector val, T min, T max, const std::string & name) { + for (size_t i = 0; i < val.size(); i++) { + std::stringstream oss; + oss << name << "[" << i << "]"; + checkRange(val[i], min, max, oss.str()); + } +} + +/** + * @brief Get a unsigned integer value from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +bool getRosUint(ros::NodeHandle* node, const std::string& key, U &u) { + int param; + if (!node->getParam(key, param)) { + return false; + } + // Check the bounds + U min = std::numeric_limits::lowest(); + U max = std::numeric_limits::max(); + checkRange(param, min, max, key); + // set the output + u = (U) param; + return true; +} + +/** + * @brief Get a unsigned integer value from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @param val value to use if the server doesn't contain this parameter. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +void getRosUint(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { + if (!getRosUint(node, key, u)) { + u = default_val; + } +} + +/** + * @brief Get a unsigned integer vector from the parameter server. + * @throws std::runtime_error if the parameter is out of bounds. + * @return true if found, false if not found. + */ +template +bool getRosUint(ros::NodeHandle* node, const std::string& key, std::vector &u) { + std::vector param; + if (!node->getParam(key, param)) { + return false; + } + + // Check the bounds + U min = std::numeric_limits::lowest(); + U max = std::numeric_limits::max(); + checkRange(param, min, max, key); + + // set the output + u.insert(u.begin(), param.begin(), param.end()); + return true; +} + +/** + * @brief Get a integer (size 8 or 16) value from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +bool getRosInt(ros::NodeHandle* node, const std::string& key, I &u) { + int param; + if (!node->getParam(key, param)) { + return false; + } + // Check the bounds + I min = std::numeric_limits::lowest(); + I max = std::numeric_limits::max(); + checkRange(param, min, max, key); + // set the output + u = (I) param; + return true; +} + +/** + * @brief Get an integer value (size 8 or 16) from the parameter server. + * @param key the key to be used in the parameter server's dictionary + * @param u storage for the retrieved value. + * @param val value to use if the server doesn't contain this parameter. + * @throws std::runtime_error if the parameter is out of bounds + * @return true if found, false if not found. + */ +template +void getRosInt(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { + if (!getRosInt(node, key, u)) { + u = default_val; + } +} + +/** + * @brief Get a int (size 8 or 16) vector from the parameter server. + * @throws std::runtime_error if the parameter is out of bounds. + * @return true if found, false if not found. + */ +template +bool getRosInt(ros::NodeHandle* node, const std::string& key, std::vector &i) { + std::vector param; + if (!node->getParam(key, param)) { + return false; + } + + // Check the bounds + I min = std::numeric_limits::lowest(); + I max = std::numeric_limits::max(); + checkRange(param, min, max, key); + + // set the output + i.insert(i.begin(), param.begin(), param.end()); + return true; +} + +static inline bool declareRosBoolean(ros::NodeHandle* node, const std::string &name, bool default_value) +{ + bool ret; + + if (!node->hasParam(name)) { + node->setParam(name, default_value); + } + // implicit else: If the ROS node already has the parameter, just leave it + + if (!node->getParam(name, ret)) { + throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); + } +} + +static inline bool getRosBoolean(ros::NodeHandle* node, const std::string &name) +{ + bool ret; + if (!node->getParam(name, ret)) { + // Note that if this is used after declareRosBoolean, this should never happen. + throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); + } + + return ret; +} + +} // namespace ublox_node + +#endif // UBLOX_GPS_UTILS_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 1a7bffa0..2f18f110 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include namespace ublox_node { @@ -1993,83 +1994,6 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { updater_->update(); } -// -// U-Blox Time Sync Products, partially implemented. -// -TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) : frame_id_(frame_id), updater_(updater), node_(node) -{ - timtm2_pub_ = - node_->advertise("timtm2", 1); - interrupt_time_pub_ = - node_->advertise("interrupt_time", 1); - rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); - rxm_raw_pub_ = node_->advertise("rxmraw", 1); -} - -void TimProduct::getRosParams() { -} - -bool TimProduct::configureUblox(std::shared_ptr gps) { - uint8_t r = 1; - // Configure the reciever - if (!gps->setUTCtime()) { - throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); - } - - if (!gps->setTimtm2(r)) { - throw std::runtime_error(std::string("Failed to Configure TIM Product")); - } - - return true; -} - -void TimProduct::subscribe(std::shared_ptr gps) { - gps->subscribe(std::bind( - &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); - - ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); - - // Subscribe to SFRBX messages - if (getRosBoolean(node_, "publish/rxm/sfrb")) { - gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, - 1); - } - - // Subscribe to RawX messages - if (getRosBoolean(node_, "publish/rxm/raw")) { - gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, - 1); - } -} - -void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - - if (getRosBoolean(node_, "publish/tim/tm2")) { - // create time ref message and put in the data - t_ref_.header.seq = m.rising_edge_count; - t_ref_.header.stamp = ros::Time::now(); - t_ref_.header.frame_id = frame_id_; - - t_ref_.time_ref = ros::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); - - std::ostringstream src; - src << "TIM" << int(m.ch); - t_ref_.source = src.str(); - - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id_; - - timtm2_pub_.publish(m); - interrupt_time_pub_.publish(t_ref_); - } - - updater_->force_update(); -} - -void TimProduct::initializeRosDiagnostics() { - updater_->force_update(); -} - } // namespace ublox_node int main(int argc, char** argv) { diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp new file mode 100644 index 00000000..942bf6d8 --- /dev/null +++ b/ublox_gps/src/tim_product.cpp @@ -0,0 +1,96 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox Time Sync Products, partially implemented. +// +TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) : frame_id_(frame_id), updater_(updater), node_(node) +{ + timtm2_pub_ = + node_->advertise("timtm2", 1); + interrupt_time_pub_ = + node_->advertise("interrupt_time", 1); + rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); + rxm_raw_pub_ = node_->advertise("rxmraw", 1); +} + +void TimProduct::getRosParams() { +} + +bool TimProduct::configureUblox(std::shared_ptr gps) { + uint8_t r = 1; + // Configure the reciever + if (!gps->setUTCtime()) { + throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time")); + } + + if (!gps->setTimtm2(r)) { + throw std::runtime_error(std::string("Failed to Configure TIM Product")); + } + + return true; +} + +void TimProduct::subscribe(std::shared_ptr gps) { + gps->subscribe(std::bind( + &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); + + ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); + + // Subscribe to SFRBX messages + if (getRosBoolean(node_, "publish/rxm/sfrb")) { + gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, + 1); + } + + // Subscribe to RawX messages + if (getRosBoolean(node_, "publish/rxm/raw")) { + gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, + 1); + } +} + +void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { + if (getRosBoolean(node_, "publish/tim/tm2")) { + // create time ref message and put in the data + t_ref_.header.seq = m.rising_edge_count; + t_ref_.header.stamp = ros::Time::now(); + t_ref_.header.frame_id = frame_id_; + + t_ref_.time_ref = ros::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); + + std::ostringstream src; + src << "TIM" << int(m.ch); + t_ref_.source = src.str(); + + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.frame_id = frame_id_; + + timtm2_pub_.publish(m); + interrupt_time_pub_.publish(t_ref_); + } + + updater_->force_update(); +} + +void TimProduct::initializeRosDiagnostics() { + updater_->force_update(); +} + +} // namespace ublox_node From 6a64946d34471c70e1aa1ced9fe9603150a5bf63 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:39:11 +0000 Subject: [PATCH 067/152] Move AdrUdrProduct class into its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- .../include/ublox_gps/adr_udr_product.hpp | 88 ++++++++ ublox_gps/include/ublox_gps/node.hpp | 66 ------ ublox_gps/src/adr_udr_product.cpp | 194 ++++++++++++++++++ ublox_gps/src/node.cpp | 171 +-------------- 5 files changed, 284 insertions(+), 237 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/adr_udr_product.hpp create mode 100644 ublox_gps/src/adr_udr_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 48989da8..0b51aad6 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp new file mode 100644 index 00000000..0c37460f --- /dev/null +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -0,0 +1,88 @@ +#ifndef UBLOX_GPS_ADR_UDR_PRODUCT_HPP +#define UBLOX_GPS_ADR_UDR_PRODUCT_HPP + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Automotive Dead Reckoning (ADR) and + * Untethered Dead Reckoning (UDR) Devices. + */ +class AdrUdrProduct final : public virtual ComponentInterface { + public: + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); + + /** + * @brief Get the ADR/UDR parameters. + * + * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. + */ + void getRosParams() override; + + /** + * @brief Configure ADR/UDR settings. + * @details Configure the use_adr setting. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Initialize the ROS diagnostics for the ADR/UDR device. + * @todo unimplemented + */ + void initializeRosDiagnostics() override { + ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", + "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); + } + + /** + * @brief Subscribe to ADR/UDR messages. + * + * @details Subscribe to NavATT, ESF and HNR messages based on user + * parameters. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Whether or not to enable dead reckoning + bool use_adr_; + + sensor_msgs::Imu imu_; + sensor_msgs::TimeReference t_ref_; + ublox_msgs::TimTM2 timtm2; + + void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); + + ros::Publisher imu_pub_; + ros::Publisher time_ref_pub_; + ros::Publisher nav_att_pub_; + ros::Publisher esf_ins_pub_; + ros::Publisher esf_meas_pub_; + ros::Publisher esf_raw_pub_; + ros::Publisher esf_status_pub_; + ros::Publisher hnr_pvt_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + + std::string frame_id_; + std::shared_ptr updater_; + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 178b88ee..5aae3155 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -753,72 +753,6 @@ class RawDataProduct final : public virtual ComponentInterface { ros::NodeHandle* node_; }; -/** - * @brief Implements functions for Automotive Dead Reckoning (ADR) and - * Untethered Dead Reckoning (UDR) Devices. - */ -class AdrUdrProduct final : public virtual ComponentInterface { - public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); - - /** - * @brief Get the ADR/UDR parameters. - * - * @details Get the use_adr parameter and check that the nav_rate is 1 Hz. - */ - void getRosParams() override; - - /** - * @brief Configure ADR/UDR settings. - * @details Configure the use_adr setting. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Initialize the ROS diagnostics for the ADR/UDR device. - * @todo unimplemented - */ - void initializeRosDiagnostics() override { - ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", - "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); - } - - /** - * @brief Subscribe to ADR/UDR messages. - * - * @details Subscribe to NavATT, ESF and HNR messages based on user - * parameters. - */ - void subscribe(std::shared_ptr gps) override; - - private: - //! Whether or not to enable dead reckoning - bool use_adr_; - - sensor_msgs::Imu imu_; - sensor_msgs::TimeReference t_ref_; - ublox_msgs::TimTM2 timtm2; - - void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); - - ros::Publisher imu_pub_; - ros::Publisher time_ref_pub_; - ros::Publisher nav_att_pub_; - ros::Publisher esf_ins_pub_; - ros::Publisher esf_meas_pub_; - ros::Publisher esf_raw_pub_; - ros::Publisher esf_status_pub_; - ros::Publisher hnr_pvt_pub_; - - uint16_t nav_rate_; - uint16_t meas_rate_; - - std::string frame_id_; - std::shared_ptr updater_; - ros::NodeHandle* node_; -}; - /** * @brief Implements functions for High Precision GNSS Reference station * devices. diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp new file mode 100644 index 00000000..77150015 --- /dev/null +++ b/ublox_gps/src/adr_udr_product.cpp @@ -0,0 +1,194 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// u-blox ADR devices, partially implemented +// +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) +{ + imu_pub_ = + node_->advertise("imu_meas", 1); + time_ref_pub_ = + node_->advertise("interrupt_time", 1); + nav_att_pub_ = node_->advertise("navatt", 1); + esf_ins_pub_ = node_->advertise("esfins", 1); + esf_meas_pub_ = node_->advertise("esfmeas", 1); + esf_raw_pub_ = node_->advertise("esfraw", 1); + esf_status_pub_ = node_->advertise("esfstatus", 1); + hnr_pvt_pub_ = node_->advertise("hnrpvt", 1); +} + +void AdrUdrProduct::getRosParams() { + use_adr_ = getRosBoolean(node_, "use_adr"); + // Check the nav rate + float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); + if (nav_rate_hz != 1) { + ROS_WARN("Nav Rate recommended to be 1 Hz"); + } +} + +bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { + if (!gps->setUseAdr(use_adr_)) { + throw std::runtime_error(std::string("Failed to ") + + (use_adr_ ? "enable" : "disable") + "use_adr"); + } + return true; +} + +void AdrUdrProduct::subscribe(std::shared_ptr gps) { + // Subscribe to NAV ATT messages + if (getRosBoolean(node_, "publish/nav/att")) { + gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, + 1); + } + + // Subscribe to ESF INS messages + if (getRosBoolean(node_, "publish/esf/ins")) { + gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, + 1); + } + + // Subscribe to ESF Meas messages + if (getRosBoolean(node_, "publish/esf/meas")) { + gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, + 1); + // also publish sensor_msgs::Imu + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); + } + + // Subscribe to ESF Raw messages + if (getRosBoolean(node_, "publish/esf/raw")) { + gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, + 1); + } + + // Subscribe to ESF Status messages + if (getRosBoolean(node_, "publish/esf/status")) { + gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, + 1); + } + + // Subscribe to HNR PVT messages + if (getRosBoolean(node_, "publish/hnr/pvt")) { + gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, + 1); + } +} + +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { + if (getRosBoolean(node_, "publish/esf/meas")) { + imu_.header.stamp = ros::Time::now(); + imu_.header.frame_id = frame_id_; + + float deg_per_sec = pow(2, -12); + float m_per_sec_sq = pow(2, -10); + float deg_c = 1e-2; + + std::vector imu_data = m.data; + for (int i = 0; i < imu_data.size(); i++){ + unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data + double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data + unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits + + if (data_sign == 0) { + data_sign = -1; + } else { + data_sign = 1; + } + + //ROS_INFO("data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray + + imu_.orientation_covariance[0] = -1; + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; + + if (data_type == 14) { + if (data_sign == 1) { + imu_.angular_velocity.x = 2048 - data_value * deg_per_sec; + } else { + imu_.angular_velocity.x = data_sign * data_value * deg_per_sec; + } + } else if (data_type == 16) { + //ROS_INFO("data_sign: %f", data_sign); + //ROS_INFO("data_value: %u", data_value * m); + if (data_sign == 1) { + imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.x = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 13) { + if (data_sign == 1) { + imu_.angular_velocity.y = 2048 - data_value * deg_per_sec; + } else { + imu_.angular_velocity.y = data_sign * data_value * deg_per_sec; + } + } else if (data_type == 17) { + if (data_sign == 1) { + imu_.linear_acceleration.y = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.y = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 5) { + if (data_sign == 1) { + imu_.angular_velocity.z = 2048 - data_value * deg_per_sec; + } else { + imu_.angular_velocity.z = data_sign * data_value * deg_per_sec; + } + } else if (data_type == 18) { + if (data_sign == 1) { + imu_.linear_acceleration.z = 8191 - data_value * m_per_sec_sq; + } else { + imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; + } + } else if (data_type == 12) { + //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); + } else { + ROS_INFO("data_type: %u", data_type); + ROS_INFO("data_value: %u", data_value); + } + + // create time ref message and put in the data + //t_ref_.header.seq = m.risingEdgeCount; + //t_ref_.header.stamp = ros::Time::now(); + //t_ref_.header.frame_id = frame_id_; + + //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + + //std::ostringstream src; + //src << "TIM" << int(m.ch); + //t_ref_.source = src.str(); + + t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.frame_id = frame_id_; + + time_ref_pub_.publish(t_ref_); + imu_pub_.publish(imu_); + } + } + + updater_->force_update(); +} + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 2f18f110..c3ee7c3f 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -38,6 +38,7 @@ #include +#include #include #include #include @@ -1476,176 +1477,6 @@ void RawDataProduct::initializeRosDiagnostics() { } } -// -// u-blox ADR devices, partially implemented -// -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) - : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) -{ - imu_pub_ = - node_->advertise("imu_meas", 1); - time_ref_pub_ = - node_->advertise("interrupt_time", 1); - nav_att_pub_ = node_->advertise("navatt", 1); - esf_ins_pub_ = node_->advertise("esfins", 1); - esf_meas_pub_ = node_->advertise("esfmeas", 1); - esf_raw_pub_ = node_->advertise("esfraw", 1); - esf_status_pub_ = node_->advertise("esfstatus", 1); - hnr_pvt_pub_ = node_->advertise("hnrpvt", 1); -} - -void AdrUdrProduct::getRosParams() { - use_adr_ = getRosBoolean(node_, "use_adr"); - // Check the nav rate - float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); - if (nav_rate_hz != 1) { - ROS_WARN("Nav Rate recommended to be 1 Hz"); - } -} - -bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { - if (!gps->setUseAdr(use_adr_)) { - throw std::runtime_error(std::string("Failed to ") - + (use_adr_ ? "enable" : "disable") + "use_adr"); - } - return true; -} - -void AdrUdrProduct::subscribe(std::shared_ptr gps) { - // Subscribe to NAV ATT messages - if (getRosBoolean(node_, "publish/nav/att")) { - gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, - 1); - } - - // Subscribe to ESF INS messages - if (getRosBoolean(node_, "publish/esf/ins")) { - gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, - 1); - } - - // Subscribe to ESF Meas messages - if (getRosBoolean(node_, "publish/esf/meas")) { - gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, - 1); - // also publish sensor_msgs::Imu - gps->subscribe(std::bind( - &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); - } - - // Subscribe to ESF Raw messages - if (getRosBoolean(node_, "publish/esf/raw")) { - gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, - 1); - } - - // Subscribe to ESF Status messages - if (getRosBoolean(node_, "publish/esf/status")) { - gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, - 1); - } - - // Subscribe to HNR PVT messages - if (getRosBoolean(node_, "publish/hnr/pvt")) { - gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, - 1); - } -} - -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (getRosBoolean(node_, "publish/esf/meas")) { - imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id_; - - float deg_per_sec = pow(2, -12); - float m_per_sec_sq = pow(2, -10); - float deg_c = 1e-2; - - std::vector imu_data = m.data; - for (int i = 0; i < imu_data.size(); i++){ - unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data - double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data - unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits - - if (data_sign == 0) { - data_sign = -1; - } else { - data_sign = 1; - } - - //ROS_INFO("data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray - - imu_.orientation_covariance[0] = -1; - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - if (data_type == 14) { - if (data_sign == 1) { - imu_.angular_velocity.x = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.x = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 16) { - //ROS_INFO("data_sign: %f", data_sign); - //ROS_INFO("data_value: %u", data_value * m); - if (data_sign == 1) { - imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.x = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 13) { - if (data_sign == 1) { - imu_.angular_velocity.y = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.y = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 17) { - if (data_sign == 1) { - imu_.linear_acceleration.y = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.y = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 5) { - if (data_sign == 1) { - imu_.angular_velocity.z = 2048 - data_value * deg_per_sec; - } else { - imu_.angular_velocity.z = data_sign * data_value * deg_per_sec; - } - } else if (data_type == 18) { - if (data_sign == 1) { - imu_.linear_acceleration.z = 8191 - data_value * m_per_sec_sq; - } else { - imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; - } - } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); - } else { - ROS_INFO("data_type: %u", data_type); - ROS_INFO("data_value: %u", data_value); - } - - // create time ref message and put in the data - //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = ros::Time::now(); - //t_ref_.header.frame_id = frame_id_; - - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); - - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp - t_ref_.header.frame_id = frame_id_; - - time_ref_pub_.publish(t_ref_); - imu_pub_.publish(imu_); - } - } - - updater_->force_update(); -} - // // u-blox High Precision GNSS Reference Station // From 0857d0fba6893176e3bf97db0983c77f673a904e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:48:29 +0000 Subject: [PATCH 068/152] Move HpgRovProduct class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- .../include/ublox_gps/hpg_rov_product.hpp | 95 ++++++++++++++++ ublox_gps/include/ublox_gps/node.hpp | 76 ------------- .../ublox_gps/ublox_topic_diagnostic.hpp | 6 +- ublox_gps/src/hpg_rov_product.cpp | 102 ++++++++++++++++++ ublox_gps/src/node.cpp | 83 +------------- 6 files changed, 204 insertions(+), 160 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/hpg_rov_product.hpp create mode 100644 ublox_gps/src/hpg_rov_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 0b51aad6..7e0e19c6 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp new file mode 100644 index 00000000..9e6b7720 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp @@ -0,0 +1,95 @@ +#ifndef UBLOX_GPS_HPG_ROV_PRODUCT_HPP +#define UBLOX_GPS_HPG_ROV_PRODUCT_HPP + +#include + +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for High Precision GNSS Rover devices. + */ +class HpgRovProduct final : public virtual ComponentInterface { + public: + // Constants for diagnostic updater + //! Diagnostic updater: RTCM topic frequency min [Hz] + constexpr static double kRtcmFreqMin = 1; + //! Diagnostic updater: RTCM topic frequency max [Hz] + constexpr static double kRtcmFreqMax = 10; + //! Diagnostic updater: RTCM topic frequency tolerance [%] + constexpr static double kRtcmFreqTol = 0.1; + //! Diagnostic updater: RTCM topic frequency window [num messages] + constexpr static int kRtcmFreqWindow = 25; + + explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node); + + /** + * @brief Get the ROS parameters specific to the Rover configuration. + * + * @details Get the DGNSS mode. + */ + void getRosParams() override; + + /** + * @brief Configure rover settings. + * + * @details Configure the DGNSS mode. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Add diagnostic updaters for rover GNSS status, including + * status of RTCM messages. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Rover messages, such as NavRELPOSNED. + */ + void subscribe(std::shared_ptr gps) override; + + private: + /** + * @brief Update the rover diagnostics, including the carrier phase solution + * status (float or fixed). + */ + void carrierPhaseDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat); + + /** + * @brief Set the last received message and call rover diagnostic updater + * + * @details Publish received NavRELPOSNED messages if enabled + */ + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); + + + //! Last relative position (used for diagnostic updater) + ublox_msgs::NavRELPOSNED last_rel_pos_; + + //! The DGNSS mode + /*! see CfgDGNSS message for possible values */ + uint8_t dgnss_mode_; + + //! The RTCM topic frequency diagnostic updater + UbloxTopicDiagnostic freq_rtcm_; + + ros::Publisher nav_rel_pos_ned_pub_; + + uint16_t nav_rate_; + std::shared_ptr updater_; + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HPG_ROV_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 5aae3155..be8e3833 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -878,82 +878,6 @@ class HpgRefProduct: public virtual ComponentInterface { ros::NodeHandle* node_; }; -/** - * @brief Implements functions for High Precision GNSS Rover devices. - */ -class HpgRovProduct final : public virtual ComponentInterface { - public: - // Constants for diagnostic updater - //! Diagnostic updater: RTCM topic frequency min [Hz] - constexpr static double kRtcmFreqMin = 1; - //! Diagnostic updater: RTCM topic frequency max [Hz] - constexpr static double kRtcmFreqMax = 10; - //! Diagnostic updater: RTCM topic frequency tolerance [%] - constexpr static double kRtcmFreqTol = 0.1; - //! Diagnostic updater: RTCM topic frequency window [num messages] - constexpr static int kRtcmFreqWindow = 25; - - explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node); - - /** - * @brief Get the ROS parameters specific to the Rover configuration. - * - * @details Get the DGNSS mode. - */ - void getRosParams() override; - - /** - * @brief Configure rover settings. - * - * @details Configure the DGNSS mode. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Add diagnostic updaters for rover GNSS status, including - * status of RTCM messages. - */ - void initializeRosDiagnostics() override; - - /** - * @brief Subscribe to Rover messages, such as NavRELPOSNED. - */ - void subscribe(std::shared_ptr gps) override; - - private: - /** - * @brief Update the rover diagnostics, including the carrier phase solution - * status (float or fixed). - */ - void carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat); - - /** - * @brief Set the last received message and call rover diagnostic updater - * - * @details Publish received NavRELPOSNED messages if enabled - */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); - - - //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED last_rel_pos_; - - //! The DGNSS mode - /*! see CfgDGNSS message for possible values */ - uint8_t dgnss_mode_; - - //! The RTCM topic frequency diagnostic updater - UbloxTopicDiagnostic freq_rtcm_; - - ros::Publisher nav_rel_pos_ned_pub_; - - uint16_t nav_rate_; - std::shared_ptr updater_; - ros::NodeHandle* node_; -}; - class HpPosRecProduct final : public virtual HpgRefProduct { public: explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); diff --git a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp index 59b49d95..c347f822 100644 --- a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp @@ -4,6 +4,10 @@ #include #include +#include +#include +#include + namespace ublox_node { //! Topic diagnostics for u-blox messages @@ -62,6 +66,6 @@ struct UbloxTopicDiagnostic { double max_freq; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp new file mode 100644 index 00000000..7c5042c6 --- /dev/null +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -0,0 +1,102 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox High Precision GNSS Rover +// +HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), updater_(updater), node_(node) +{ + nav_rel_pos_ned_pub_ = + node_->advertise("navrelposned", 1); +} + +void HpgRovProduct::getRosParams() { + // default to float, see CfgDGNSS message for details + getRosUint(node_, "dgnss_mode", dgnss_mode_, + ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); +} + +bool HpgRovProduct::configureUblox(std::shared_ptr gps) { + // Configure the DGNSS + if (!gps->setDgnss(dgnss_mode_)) { + throw std::runtime_error(std::string("Failed to Configure DGNSS")); + } + return true; +} + +void HpgRovProduct::subscribe(std::shared_ptr gps) { + // Subscribe to Nav Relative Position NED messages (also updates diagnostics) + gps->subscribe(std::bind( + &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); +} + +void HpgRovProduct::initializeRosDiagnostics() { + freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), + kRtcmFreqMin, kRtcmFreqMax, + kRtcmFreqTol, kRtcmFreqWindow, updater_); + updater_->add("Carrier Phase Solution", this, + &HpgRovProduct::carrierPhaseDiagnostics); + updater_->force_update(); +} + +void HpgRovProduct::carrierPhaseDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; + stat.add("iTow", last_rel_pos_.i_tow); + if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || + !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && + last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { + stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.message = "None"; + } else { + if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "Float"; + } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Fixed"; + } + stat.add("Ref Station ID", last_rel_pos_.ref_station_id); + + double rel_pos_n = (last_rel_pos_.rel_pos_n + + (last_rel_pos_.rel_pos_hpn * 1e-2)) * 1e-2; + double rel_pos_e = (last_rel_pos_.rel_pos_e + + (last_rel_pos_.rel_pos_hpe * 1e-2)) * 1e-2; + double rel_pos_d = (last_rel_pos_.rel_pos_d + + (last_rel_pos_.rel_pos_hpd * 1e-2)) * 1e-2; + stat.add("Relative Position N [m]", rel_pos_n); + stat.add("Relative Accuracy N [m]", last_rel_pos_.acc_n * 1e-4); + stat.add("Relative Position E [m]", rel_pos_e); + stat.add("Relative Accuracy E [m]", last_rel_pos_.acc_e * 1e-4); + stat.add("Relative Position D [m]", rel_pos_d); + stat.add("Relative Accuracy D [m]", last_rel_pos_.acc_d * 1e-4); + } +} + +void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { + if (getRosBoolean(node_, "publish/nav/relposned")) { + nav_rel_pos_ned_pub_.publish(m); + } + + last_rel_pos_ = m; + updater_->update(); +} + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index c3ee7c3f..94f8162e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -1692,88 +1693,6 @@ void HpgRefProduct::tmode3Diagnostics( } } -// -// U-Blox High Precision GNSS Rover -// -HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node) - : nav_rate_(nav_rate), updater_(updater), node_(node) -{ - nav_rel_pos_ned_pub_ = - node_->advertise("navrelposned", 1); -} - -void HpgRovProduct::getRosParams() { - // default to float, see CfgDGNSS message for details - getRosUint(node_, "dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); -} - -bool HpgRovProduct::configureUblox(std::shared_ptr gps) { - // Configure the DGNSS - if (!gps->setDgnss(dgnss_mode_)) { - throw std::runtime_error(std::string("Failed to Configure DGNSS")); - } - return true; -} - -void HpgRovProduct::subscribe(std::shared_ptr gps) { - // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps->subscribe(std::bind( - &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); -} - -void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow, updater_); - updater_->add("Carrier Phase Solution", this, - &HpgRovProduct::carrierPhaseDiagnostics); - updater_->force_update(); -} - -void HpgRovProduct::carrierPhaseDiagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; - stat.add("iTow", last_rel_pos_.i_tow); - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || - !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && - last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "None"; - } else { - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Float"; - } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Fixed"; - } - stat.add("Ref Station ID", last_rel_pos_.ref_station_id); - - double rel_pos_n = (last_rel_pos_.rel_pos_n - + (last_rel_pos_.rel_pos_hpn * 1e-2)) * 1e-2; - double rel_pos_e = (last_rel_pos_.rel_pos_e - + (last_rel_pos_.rel_pos_hpe * 1e-2)) * 1e-2; - double rel_pos_d = (last_rel_pos_.rel_pos_d - + (last_rel_pos_.rel_pos_hpd * 1e-2)) * 1e-2; - stat.add("Relative Position N [m]", rel_pos_n); - stat.add("Relative Accuracy N [m]", last_rel_pos_.acc_n * 1e-4); - stat.add("Relative Position E [m]", rel_pos_e); - stat.add("Relative Accuracy E [m]", last_rel_pos_.acc_e * 1e-4); - stat.add("Relative Position D [m]", rel_pos_d); - stat.add("Relative Accuracy D [m]", last_rel_pos_.acc_d * 1e-4); - } -} - -void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (getRosBoolean(node_, "publish/nav/relposned")) { - nav_rel_pos_ned_pub_.publish(m); - } - - last_rel_pos_ = m; - updater_->update(); -} - // // U-Blox High Precision Positioning Receiver // From 46266be77d3a1a8a1d031731973957f0929f78e3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:53:37 +0000 Subject: [PATCH 069/152] Move RawDataProduct class into its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 51 ------------- .../include/ublox_gps/raw_data_product.hpp | 69 +++++++++++++++++ ublox_gps/src/node.cpp | 57 +------------- ublox_gps/src/raw_data_product.cpp | 74 +++++++++++++++++++ 5 files changed, 145 insertions(+), 108 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/raw_data_product.hpp create mode 100644 ublox_gps/src/raw_data_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 7e0e19c6..eebd8f0d 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index be8e3833..37e2c05c 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -702,57 +702,6 @@ class UbloxFirmware9 final : public UbloxFirmware8 { explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); }; -/** - * @brief Implements functions for Raw Data products. - */ -class RawDataProduct final : public virtual ComponentInterface { - public: - double kRtcmFreqTol = 0.15; - int kRtcmFreqWindow = 25; - - explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node); - - /** - * @brief Does nothing since there are no Raw Data product specific settings. - */ - void getRosParams() override {} - - /** - * @brief Does nothing since there are no Raw Data product specific settings. - * @return always returns true - */ - bool configureUblox(std::shared_ptr gps) override { - (void)gps; - return true; - } - - /** - * @brief Adds frequency diagnostics for RTCM topics. - */ - void initializeRosDiagnostics() override; - - /** - * @brief Subscribe to Raw Data Product messages and set up ROS publishers. - * - * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. - */ - void subscribe(std::shared_ptr gps) override; - - private: - //! Topic diagnostic updaters - std::vector > freq_diagnostics_; - - ros::Publisher rxm_raw_pub_; - ros::Publisher rxm_sfrb_pub_; - ros::Publisher rxm_eph_pub_; - ros::Publisher rxm_alm_pub_; - - uint16_t nav_rate_; - uint16_t meas_rate_; - std::shared_ptr updater_; - ros::NodeHandle* node_; -}; - /** * @brief Implements functions for High Precision GNSS Reference station * devices. diff --git a/ublox_gps/include/ublox_gps/raw_data_product.hpp b/ublox_gps/include/ublox_gps/raw_data_product.hpp new file mode 100644 index 00000000..57e7d9fd --- /dev/null +++ b/ublox_gps/include/ublox_gps/raw_data_product.hpp @@ -0,0 +1,69 @@ +#ifndef UBLOX_GPS_RAW_DATA_PRODUCT_HPP +#define UBLOX_GPS_RAW_DATA_PRODUCT_HPP + +#include +#include + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for Raw Data products. + */ +class RawDataProduct final : public virtual ComponentInterface { + public: + double kRtcmFreqTol = 0.15; + int kRtcmFreqWindow = 25; + + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node); + + /** + * @brief Does nothing since there are no Raw Data product specific settings. + */ + void getRosParams() override {} + + /** + * @brief Does nothing since there are no Raw Data product specific settings. + * @return always returns true + */ + bool configureUblox(std::shared_ptr gps) override { + (void)gps; + return true; + } + + /** + * @brief Adds frequency diagnostics for RTCM topics. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to Raw Data Product messages and set up ROS publishers. + * + * @details Subscribe to RxmALM, RxmEPH, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Topic diagnostic updaters + std::vector > freq_diagnostics_; + + ros::Publisher rxm_raw_pub_; + ros::Publisher rxm_sfrb_pub_; + ros::Publisher rxm_eph_pub_; + ros::Publisher rxm_alm_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + std::shared_ptr updater_; + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_RAW_DATA_PRODUCT_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 94f8162e..d02e9aa8 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -1422,62 +1423,6 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) - : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { - rxm_raw_pub_ = node_->advertise("rxmraw", 1); - rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); - rxm_eph_pub_ = node_->advertise("rxmeph", 1); - rxm_alm_pub_ = node_->advertise("rxmalm", 1); -} - -void RawDataProduct::subscribe(std::shared_ptr gps) { - // Subscribe to RXM Raw - if (getRosBoolean(node_, "publish/rxm/raw")) { - gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, - 1); - } - - // Subscribe to RXM SFRB - if (getRosBoolean(node_, "publish/rxm/sfrb")) { - gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, - 1); - } - - // Subscribe to RXM EPH - if (getRosBoolean(node_, "publish/rxm/eph")) { - gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, - 1); - } - - // Subscribe to RXM ALM - if (getRosBoolean(node_, "publish/rxm/almRaw")) { - gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, - 1); - } -} - -void RawDataProduct::initializeRosDiagnostics() { - if (getRosBoolean(node_, "publish/rxm/raw")) { - freq_diagnostics_.push_back(std::make_shared( - "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); - } - if (getRosBoolean(node_, "publish/rxm/sfrb")) { - freq_diagnostics_.push_back(std::make_shared( - "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); - } - if (getRosBoolean(node_, "publish/rxm/eph")) { - freq_diagnostics_.push_back(std::make_shared( - "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); - } - if (getRosBoolean(node_, "publish/rxm/almRaw")) { - freq_diagnostics_.push_back(std::make_shared( - "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); - } -} - // // u-blox High Precision GNSS Reference Station // diff --git a/ublox_gps/src/raw_data_product.cpp b/ublox_gps/src/raw_data_product.cpp new file mode 100644 index 00000000..1db9d167 --- /dev/null +++ b/ublox_gps/src/raw_data_product.cpp @@ -0,0 +1,74 @@ +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +// +// Raw Data Products +// +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { + rxm_raw_pub_ = node_->advertise("rxmraw", 1); + rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); + rxm_eph_pub_ = node_->advertise("rxmeph", 1); + rxm_alm_pub_ = node_->advertise("rxmalm", 1); +} + +void RawDataProduct::subscribe(std::shared_ptr gps) { + // Subscribe to RXM Raw + if (getRosBoolean(node_, "publish/rxm/raw")) { + gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, + 1); + } + + // Subscribe to RXM SFRB + if (getRosBoolean(node_, "publish/rxm/sfrb")) { + gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, + 1); + } + + // Subscribe to RXM EPH + if (getRosBoolean(node_, "publish/rxm/eph")) { + gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, + 1); + } + + // Subscribe to RXM ALM + if (getRosBoolean(node_, "publish/rxm/almRaw")) { + gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, + 1); + } +} + +void RawDataProduct::initializeRosDiagnostics() { + if (getRosBoolean(node_, "publish/rxm/raw")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish/rxm/sfrb")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish/rxm/eph")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } + if (getRosBoolean(node_, "publish/rxm/almRaw")) { + freq_diagnostics_.push_back(std::make_shared( + "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); + } +} + +} // namespace ublox_node From f0ee947e71a98c8d2e37021cbdd43cbec83c5940 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 14:59:23 +0000 Subject: [PATCH 070/152] Move UbloxFirmware class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 33 +------------ .../include/ublox_gps/ublox_firmware.hpp | 48 +++++++++++++++++++ ublox_gps/src/node.cpp | 13 +---- ublox_gps/src/ublox_firmware.cpp | 23 +++++++++ 5 files changed, 74 insertions(+), 45 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware.hpp create mode 100644 ublox_gps/src/ublox_firmware.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index eebd8f0d..8aa7a855 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 37e2c05c..17644375 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include // This file also declares UbloxNode which implements ComponentInterface and is @@ -292,38 +293,6 @@ class UbloxNode final { }; -/** - * @brief This abstract class represents a firmware component. - * - * @details The Firmware components update the fix diagnostics. - */ -class UbloxFirmware : public virtual ComponentInterface { - public: - //! Subscribe Rate for u-blox SV Info messages - constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - - explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node); - - /** - * @brief Add the fix diagnostics to the updater. - */ - void initializeRosDiagnostics() override; - - protected: - /** - * @brief Handle to send fix status to ROS diagnostics. - */ - virtual void fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; - - std::shared_ptr updater_; - std::shared_ptr gnss_; - //! The fix status service type, set in the Firmware Component - //! based on the enabled GNSS - int fix_status_service_{0}; - ros::NodeHandle* node_; -}; - /** * @brief Implements functions for firmware version 6. */ diff --git a/ublox_gps/include/ublox_gps/ublox_firmware.hpp b/ublox_gps/include/ublox_gps/ublox_firmware.hpp new file mode 100644 index 00000000..f8a6725e --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware.hpp @@ -0,0 +1,48 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE_HPP + +#include + +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief This abstract class represents a firmware component. + * + * @details The Firmware components update the fix diagnostics. + */ +class UbloxFirmware : public virtual ComponentInterface { + public: + //! Subscribe Rate for u-blox SV Info messages + constexpr static uint32_t kNavSvInfoSubscribeRate = 20; + + explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node); + + /** + * @brief Add the fix diagnostics to the updater. + */ + void initializeRosDiagnostics() override; + + protected: + /** + * @brief Handle to send fix status to ROS diagnostics. + */ + virtual void fixDiagnostic( + diagnostic_updater::DiagnosticStatusWrapper& stat) = 0; + + std::shared_ptr updater_; + std::shared_ptr gnss_; + //! The fix status service type, set in the Firmware Component + //! based on the enabled GNSS + int fix_status_service_{0}; + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d02e9aa8..0c803ddb 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include namespace ublox_node { @@ -778,18 +779,6 @@ void UbloxNode::shutdown() { } } -// -// U-Blox Firmware (all versions) -// -UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node) : updater_(updater), gnss_(gnss), node_(node) -{ -} - -void UbloxFirmware::initializeRosDiagnostics() { - updater_->add("fix", this, &UbloxFirmware::fixDiagnostic); - updater_->force_update(); -} - // // U-Blox Firmware Version 6 // diff --git a/ublox_gps/src/ublox_firmware.cpp b/ublox_gps/src/ublox_firmware.cpp new file mode 100644 index 00000000..85c0ae99 --- /dev/null +++ b/ublox_gps/src/ublox_firmware.cpp @@ -0,0 +1,23 @@ +#include + +#include +#include + +#include +#include + +namespace ublox_node { + +// +// U-Blox Firmware (all versions) +// +UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node) : updater_(updater), gnss_(gnss), node_(node) +{ +} + +void UbloxFirmware::initializeRosDiagnostics() { + updater_->add("fix", this, &UbloxFirmware::fixDiagnostic); + updater_->force_update(); +} + +} // namespace ublox_node From ace52286053944996df53808b037b1b04e169001 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:07:56 +0000 Subject: [PATCH 071/152] Move UbloxFirmware6 class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 82 ------ .../include/ublox_gps/ublox_firmware6.hpp | 107 ++++++++ ublox_gps/src/node.cpp | 214 +--------------- ublox_gps/src/ublox_firmware6.cpp | 240 ++++++++++++++++++ 5 files changed, 349 insertions(+), 296 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware6.hpp create mode 100644 ublox_gps/src/ublox_firmware6.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 8aa7a855..f4be514a 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 17644375..3718f03d 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -293,88 +293,6 @@ class UbloxNode final { }; -/** - * @brief Implements functions for firmware version 6. - */ -class UbloxFirmware6 final : public UbloxFirmware { - public: - explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); - - /** - * @brief Sets the fix status service type to GPS. - */ - void getRosParams() override; - - /** - * @brief Prints a warning, GNSS configuration not available in this version. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. - */ - void subscribe(std::shared_ptr gps) override; - - protected: - /** - * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL - * messages. - */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override; - - private: - /** - * @brief Publish the fix and call the fix diagnostic updater. - * - * @details Also updates the last known position and publishes the NavPosLLH - * message if publishing is enabled. - * @param m the message to process - */ - void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m); - - /** - * @brief Update the last known velocity. - * - * @details Publish the message if publishing is enabled. - * @param m the message to process - */ - void callbackNavVelNed(const ublox_msgs::NavVELNED& m); - - /** - * @brief Update the number of SVs used for the fix. - * - * @details Publish the message if publishing is enabled. - * @param m the message to process - */ - void callbackNavSol(const ublox_msgs::NavSOL& m); - - //! The last received navigation position - ublox_msgs::NavPOSLLH last_nav_pos_; - //! The last received navigation velocity - ublox_msgs::NavVELNED last_nav_vel_; - //! The last received num SVs used - ublox_msgs::NavSOL last_nav_sol_; - //! The last NavSatFix based on last_nav_pos_ - sensor_msgs::NavSatFix fix_; - //! The last Twist based on last_nav_vel_ - geometry_msgs::TwistWithCovarianceStamped velocity_; - - //! Used to configure NMEA (if set_nmea_) filled with ROS parameters - ublox_msgs::CfgNMEA6 cfg_nmea_; - - ros::Publisher nav_pos_llh_pub_; - ros::Publisher fix_pub_; - ros::Publisher nav_vel_ned_pub_; - ros::Publisher vel_pub_; - ros::Publisher nav_sol_pub_; - ros::Publisher nav_svinfo_pub_; - ros::Publisher mon_hw_pub_; - - std::string frame_id_; - std::shared_ptr freq_diag_; -}; - /** * @brief Abstract class for Firmware versions >= 7. * diff --git a/ublox_gps/include/ublox_gps/ublox_firmware6.hpp b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp new file mode 100644 index 00000000..f16b4414 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp @@ -0,0 +1,107 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE6_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE6_HPP + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 6. + */ +class UbloxFirmware6 final : public UbloxFirmware { + public: + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); + + /** + * @brief Sets the fix status service type to GPS. + */ + void getRosParams() override; + + /** + * @brief Prints a warning, GNSS configuration not available in this version. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to NavPVT, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + protected: + /** + * @brief Updates fix diagnostic from NavPOSLLH, NavVELNED, and NavSOL + * messages. + */ + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override; + + private: + /** + * @brief Publish the fix and call the fix diagnostic updater. + * + * @details Also updates the last known position and publishes the NavPosLLH + * message if publishing is enabled. + * @param m the message to process + */ + void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m); + + /** + * @brief Update the last known velocity. + * + * @details Publish the message if publishing is enabled. + * @param m the message to process + */ + void callbackNavVelNed(const ublox_msgs::NavVELNED& m); + + /** + * @brief Update the number of SVs used for the fix. + * + * @details Publish the message if publishing is enabled. + * @param m the message to process + */ + void callbackNavSol(const ublox_msgs::NavSOL& m); + + //! The last received navigation position + ublox_msgs::NavPOSLLH last_nav_pos_; + //! The last received navigation velocity + ublox_msgs::NavVELNED last_nav_vel_; + //! The last received num SVs used + ublox_msgs::NavSOL last_nav_sol_; + //! The last NavSatFix based on last_nav_pos_ + sensor_msgs::NavSatFix fix_; + //! The last Twist based on last_nav_vel_ + geometry_msgs::TwistWithCovarianceStamped velocity_; + + //! Used to configure NMEA (if set_nmea_) filled with ROS parameters + ublox_msgs::CfgNMEA6 cfg_nmea_; + + ros::Publisher nav_pos_llh_pub_; + ros::Publisher fix_pub_; + ros::Publisher nav_vel_ned_pub_; + ros::Publisher vel_pub_; + ros::Publisher nav_sol_pub_; + ros::Publisher nav_svinfo_pub_; + ros::Publisher mon_hw_pub_; + + std::string frame_id_; + std::shared_ptr freq_diag_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE6_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 0c803ddb..91ecf1f3 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include namespace ublox_node { @@ -779,219 +780,6 @@ void UbloxNode::shutdown() { } } -// -// U-Blox Firmware Version 6 -// -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) -{ - nav_pos_llh_pub_ = - node_->advertise("navposllh", 1); - fix_pub_ = - node_->advertise("fix", 1); - - nav_vel_ned_pub_ = - node_->advertise("navvelned", 1); - - vel_pub_ = - node_->advertise("fix_velocity", - 1); - - nav_sol_pub_ = - node_->advertise("navsol", 1); - - nav_svinfo_pub_ = - node_->advertise("navinfo", 1); - - mon_hw_pub_ = - node_->advertise("monhw", 1); -} - -void UbloxFirmware6::getRosParams() { - // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; - - if (getRosBoolean(node_, "nmea/set")) { - bool compat, consider; - - if (!getRosUint(node_, "nmea/version", cfg_nmea_.version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - } - - // set flags - cfg_nmea_.flags = getRosBoolean(node_, "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; - - // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; - } -} - -bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { - ROS_WARN("ublox_version < 7, ignoring GNSS settings"); - - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { - throw std::runtime_error("Failed to configure NMEA"); - } - - return true; -} - -void UbloxFirmware6::subscribe(std::shared_ptr gps) { - // Always subscribes to these messages, but may not publish to ROS topic - // Subscribe to Nav POSLLH - gps->subscribe(std::bind( - &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), 1); - gps->subscribe(std::bind( - // Subscribe to Nav SOL - &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), 1); - // Subscribe to Nav VELNED - gps->subscribe(std::bind( - &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); - - // Subscribe to Nav SVINFO - if (getRosBoolean(node_, "publish/nav/svinfo")) { - gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, - kNavSvInfoSubscribeRate); - } - - // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, - 1); - } -} - -void UbloxFirmware6::fixDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - // Set the diagnostic level based on the fix status - if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_2D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "2D fix"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_3D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "3D fix"; - } else if (last_nav_sol_.gps_fix == - ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time fix only"; - } - // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message += ", fix not ok"; - } - // Raise diagnostic level to error if no fix - if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "No fix"; - } - - // Add last fix position - stat.add("iTOW [ms]", last_nav_pos_.i_tow); - stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7); - stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7); - stat.add("Altitude [m]", last_nav_pos_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pos_.h_msl * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pos_.h_acc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pos_.v_acc * 1e-3); - stat.add("# SVs used", (int)last_nav_sol_.num_sv); -} - -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if (getRosBoolean(node_, "publish/nav/posllh")) { - nav_pos_llh_pub_.publish(m); - } - - // Position message - if (m.i_tow == last_nav_vel_.i_tow) { - fix_.header.stamp = velocity_.header.stamp; // use last timestamp - } else { - fix_.header.stamp = ros::Time::now(); // new timestamp - } - - fix_.header.frame_id = frame_id_; - fix_.latitude = m.lat * 1e-7; - fix_.longitude = m.lon * 1e-7; - fix_.altitude = m.height * 1e-3; - - if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) { - fix_.status.status = fix_.status.STATUS_FIX; - } else { - fix_.status.status = fix_.status.STATUS_NO_FIX; - } - - // Convert from mm to m - const double var_h = pow(m.h_acc / 1000.0, 2); - const double var_v = pow(m.v_acc / 1000.0, 2); - - fix_.position_covariance[0] = var_h; - fix_.position_covariance[4] = var_h; - fix_.position_covariance[8] = var_v; - fix_.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - - fix_.status.service = fix_.status.SERVICE_GPS; - fix_pub_.publish(fix_); - last_nav_pos_ = m; - // update diagnostics - freq_diag_->diagnostic->tick(fix_.header.stamp); - updater_->update(); -} - -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if (getRosBoolean(node_, "publish/nav/velned")) { - nav_vel_ned_pub_.publish(m); - } - - // Example geometry message - if (m.i_tow == last_nav_pos_.i_tow) { - velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh - } else { - velocity_.header.stamp = ros::Time::now(); // create a new timestamp - } - velocity_.header.frame_id = frame_id_; - - // convert to XYZ linear velocity - velocity_.twist.twist.linear.x = m.vel_e / 100.0; - velocity_.twist.twist.linear.y = m.vel_n / 100.0; - velocity_.twist.twist.linear.z = -m.vel_d / 100.0; - - const double var_speed = pow(m.s_acc / 100.0, 2); - - const int cols = 6; - velocity_.twist.covariance[cols * 0 + 0] = var_speed; - velocity_.twist.covariance[cols * 1 + 1] = var_speed; - velocity_.twist.covariance[cols * 2 + 2] = var_speed; - velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - - vel_pub_.publish(velocity_); - last_nav_vel_ = m; -} - -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if (getRosBoolean(node_, "publish/nav/sol")) { - nav_sol_pub_.publish(m); - } - last_nav_sol_ = m; -} - // // Ublox Firmware Version 7 // diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp new file mode 100644 index 00000000..c0206ea5 --- /dev/null +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -0,0 +1,240 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox Firmware Version 6 +// +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) +{ + nav_pos_llh_pub_ = + node_->advertise("navposllh", 1); + fix_pub_ = + node_->advertise("fix", 1); + + nav_vel_ned_pub_ = + node_->advertise("navvelned", 1); + + vel_pub_ = + node_->advertise("fix_velocity", + 1); + + nav_sol_pub_ = + node_->advertise("navsol", 1); + + nav_svinfo_pub_ = + node_->advertise("navinfo", 1); + + mon_hw_pub_ = + node_->advertise("monhw", 1); +} + +void UbloxFirmware6::getRosParams() { + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; + + if (getRosBoolean(node_, "nmea/set")) { + bool compat, consider; + + if (!getRosUint(node_, "nmea/version", cfg_nmea_.version)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/version must be set"); + } + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/num_sv must be set"); + } + + // set flags + cfg_nmea_.flags = getRosBoolean(node_, "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; + + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + } +} + +bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { + ROS_WARN("ublox_version < 7, ignoring GNSS settings"); + + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware6::subscribe(std::shared_ptr gps) { + // Always subscribes to these messages, but may not publish to ROS topic + // Subscribe to Nav POSLLH + gps->subscribe(std::bind( + &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), 1); + gps->subscribe(std::bind( + // Subscribe to Nav SOL + &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), 1); + // Subscribe to Nav VELNED + gps->subscribe(std::bind( + &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); + + // Subscribe to Nav SVINFO + if (getRosBoolean(node_, "publish/nav/svinfo")) { + gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish/mon/hw")) { + gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, + 1); + } +} + +void UbloxFirmware6::fixDiagnostic( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + // Set the diagnostic level based on the fix status + if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "Dead reckoning only"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_2D_FIX) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "2D fix"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_3D_FIX) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "3D fix"; + } else if (last_nav_sol_.gps_fix == + ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "GPS and dead reckoning combined"; + } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Time fix only"; + } + // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level + if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message += ", fix not ok"; + } + // Raise diagnostic level to error if no fix + if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_NO_FIX) { + stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.message = "No fix"; + } + + // Add last fix position + stat.add("iTOW [ms]", last_nav_pos_.i_tow); + stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7); + stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7); + stat.add("Altitude [m]", last_nav_pos_.height * 1e-3); + stat.add("Height above MSL [m]", last_nav_pos_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pos_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pos_.v_acc * 1e-3); + stat.add("# SVs used", (int)last_nav_sol_.num_sv); +} + +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { + if (getRosBoolean(node_, "publish/nav/posllh")) { + nav_pos_llh_pub_.publish(m); + } + + // Position message + if (m.i_tow == last_nav_vel_.i_tow) { + fix_.header.stamp = velocity_.header.stamp; // use last timestamp + } else { + fix_.header.stamp = ros::Time::now(); // new timestamp + } + + fix_.header.frame_id = frame_id_; + fix_.latitude = m.lat * 1e-7; + fix_.longitude = m.lon * 1e-7; + fix_.altitude = m.height * 1e-3; + + if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) { + fix_.status.status = fix_.status.STATUS_FIX; + } else { + fix_.status.status = fix_.status.STATUS_NO_FIX; + } + + // Convert from mm to m + const double var_h = pow(m.h_acc / 1000.0, 2); + const double var_v = pow(m.v_acc / 1000.0, 2); + + fix_.position_covariance[0] = var_h; + fix_.position_covariance[4] = var_h; + fix_.position_covariance[8] = var_v; + fix_.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_.status.service = fix_.status.SERVICE_GPS; + fix_pub_.publish(fix_); + last_nav_pos_ = m; + // update diagnostics + freq_diag_->diagnostic->tick(fix_.header.stamp); + updater_->update(); +} + +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { + if (getRosBoolean(node_, "publish/nav/velned")) { + nav_vel_ned_pub_.publish(m); + } + + // Example geometry message + if (m.i_tow == last_nav_pos_.i_tow) { + velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh + } else { + velocity_.header.stamp = ros::Time::now(); // create a new timestamp + } + velocity_.header.frame_id = frame_id_; + + // convert to XYZ linear velocity + velocity_.twist.twist.linear.x = m.vel_e / 100.0; + velocity_.twist.twist.linear.y = m.vel_n / 100.0; + velocity_.twist.twist.linear.z = -m.vel_d / 100.0; + + const double var_speed = pow(m.s_acc / 100.0, 2); + + const int cols = 6; + velocity_.twist.covariance[cols * 0 + 0] = var_speed; + velocity_.twist.covariance[cols * 1 + 1] = var_speed; + velocity_.twist.covariance[cols * 2 + 2] = var_speed; + velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + + vel_pub_.publish(velocity_); + last_nav_vel_ = m; +} + +void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { + if (getRosBoolean(node_, "publish/nav/sol")) { + nav_sol_pub_.publish(m); + } + last_nav_sol_ = m; +} + +} // namespace ublox_node From 5962bb863eb02f7f190ed63f6819999ca037d36f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:12:30 +0000 Subject: [PATCH 072/152] Move UbloxFirmware7Plus class to its own header file. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 190 +--------------- .../include/ublox_gps/ublox_firmware7plus.hpp | 213 ++++++++++++++++++ ublox_gps/src/node.cpp | 1 + 3 files changed, 215 insertions(+), 189 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 3718f03d..ab3e6ee9 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include // This file also declares UbloxNode which implements ComponentInterface and is @@ -293,195 +294,6 @@ class UbloxNode final { }; -/** - * @brief Abstract class for Firmware versions >= 7. - * - * @details This class keeps track of the last NavPVT message uses it to - * update the fix diagnostics. It is a template class because the NavPVT message - * is a different length for firmware versions 7 and 8. - * - * @typedef NavPVT the NavPVT message type for the given firmware version - */ -template -class UbloxFirmware7Plus : public UbloxFirmware { - public: - explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { - // NavPVT publisher - nav_pvt_pub_ = node_->advertise("navpvt", 1); - - fix_pub_ = - node_->advertise("fix", 1); - vel_pub_ = - node_->advertise("fix_velocity", - 1); - } - - /** - * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. - * - * @details If a fixed carrier phase solution is available, the NavSatFix - * status is set to GBAS fixed. If NavPVT publishing is enabled, the message - * is published. This function also calls the ROS diagnostics updater. - * @param m the message to publish - */ - void callbackNavPvt(const NavPVT& m) { - if (getRosBoolean(node_, "publish/nav/pvt")) { - // NavPVT publisher - nav_pvt_pub_.publish(m); - } - - // - // NavSatFix message - // - sensor_msgs::NavSatFix fix; - fix.header.frame_id = frame_id_; - // set the timestamp - uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; - if (((m.valid & valid_time) == valid_time) && - (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { - // Use NavPVT timestamp since it is valid - // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 - // The ros time uses only unsigned values, so a negative nano seconds must be - // converted to a positive value - if (m.nano < 0) { - fix.header.stamp.sec = toUtcSeconds(m) - 1; - fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); - } - else { - fix.header.stamp.sec = toUtcSeconds(m); - fix.header.stamp.nsec = (uint32_t)(m.nano); - } - } else { - // Use ROS time since NavPVT timestamp is not valid - fix.header.stamp = ros::Time::now(); - } - // Set the LLA - fix.latitude = m.lat * 1e-7; // to deg - fix.longitude = m.lon * 1e-7; // to deg - fix.altitude = m.height * 1e-3; // to [m] - // Set the Fix status - bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; - if (fixOk && m.fix_type >= m.FIX_TYPE_2D) { - fix.status.status = fix.status.STATUS_FIX; - if (m.flags & m.CARRIER_PHASE_FIXED) { - fix.status.status = fix.status.STATUS_GBAS_FIX; - } - } else { - fix.status.status = fix.status.STATUS_NO_FIX; - } - // Set the service based on GNSS configuration - fix.status.service = fix_status_service_; - - // Set the position covariance - const double var_h = pow(m.h_acc / 1000.0, 2); // to [m^2] - const double var_v = pow(m.v_acc / 1000.0, 2); // to [m^2] - fix.position_covariance[0] = var_h; - fix.position_covariance[4] = var_h; - fix.position_covariance[8] = var_v; - fix.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - - fix_pub_.publish(fix); - - // - // Twist message - // - geometry_msgs::TwistWithCovarianceStamped velocity; - velocity.header.stamp = fix.header.stamp; - velocity.header.frame_id = frame_id_; - - // convert to XYZ linear velocity [m/s] in ENU - velocity.twist.twist.linear.x = m.vel_e * 1e-3; - velocity.twist.twist.linear.y = m.vel_n * 1e-3; - velocity.twist.twist.linear.z = -m.vel_d * 1e-3; - // Set the covariance - const double cov_speed = pow(m.s_acc * 1e-3, 2); - const int cols = 6; - velocity.twist.covariance[cols * 0 + 0] = cov_speed; - velocity.twist.covariance[cols * 1 + 1] = cov_speed; - velocity.twist.covariance[cols * 2 + 2] = cov_speed; - velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - - vel_pub_.publish(velocity); - - // - // Update diagnostics - // - last_nav_pvt_ = m; - freq_diag_->diagnostic->tick(fix.header.stamp); - updater_->update(); - } - - protected: - - /** - * @brief Update the fix diagnostics from Nav PVT message. - */ - void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override { - // check the last message, convert to diagnostic - if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_2D) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "2D fix"; - } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_3D) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "3D fix"; - } else if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time only fix"; - } - - // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message += ", fix not ok"; - } - // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "No fix"; - } - - // append last fix position - stat.add("iTOW [ms]", last_nav_pvt_.i_tow); - stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7); - stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7); - stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3); - stat.add("Height above MSL [m]", last_nav_pvt_.h_msl * 1e-3); - stat.add("Horizontal Accuracy [m]", last_nav_pvt_.h_acc * 1e-3); - stat.add("Vertical Accuracy [m]", last_nav_pvt_.v_acc * 1e-3); - stat.add("# SVs used", (int)last_nav_pvt_.num_sv); - } - - //! The last received NavPVT message - NavPVT last_nav_pvt_; - // Whether or not to enable the given GNSS - //! Whether or not to enable GPS - bool enable_gps_; - //! Whether or not to enable GLONASS - bool enable_glonass_; - //! Whether or not to enable QZSS - bool enable_qzss_; - //! The QZSS Signal configuration, see CfgGNSS message - uint32_t qzss_sig_cfg_; - - ros::Publisher nav_pvt_pub_; - ros::Publisher fix_pub_; - ros::Publisher vel_pub_; - - std::string frame_id_; - std::shared_ptr freq_diag_; -}; - /** * @brief Implements functions for firmware version 7. */ diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp new file mode 100644 index 00000000..6537c1e2 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -0,0 +1,213 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Abstract class for Firmware versions >= 7. + * + * @details This class keeps track of the last NavPVT message uses it to + * update the fix diagnostics. It is a template class because the NavPVT message + * is a different length for firmware versions 7 and 8. + * + * @typedef NavPVT the NavPVT message type for the given firmware version + */ +template +class UbloxFirmware7Plus : public UbloxFirmware { + public: + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { + // NavPVT publisher + nav_pvt_pub_ = node_->advertise("navpvt", 1); + + fix_pub_ = + node_->advertise("fix", 1); + vel_pub_ = + node_->advertise("fix_velocity", + 1); + } + + /** + * @brief Publish a NavSatFix and TwistWithCovarianceStamped messages. + * + * @details If a fixed carrier phase solution is available, the NavSatFix + * status is set to GBAS fixed. If NavPVT publishing is enabled, the message + * is published. This function also calls the ROS diagnostics updater. + * @param m the message to publish + */ + void callbackNavPvt(const NavPVT& m) { + if (getRosBoolean(node_, "publish/nav/pvt")) { + // NavPVT publisher + nav_pvt_pub_.publish(m); + } + + // + // NavSatFix message + // + sensor_msgs::NavSatFix fix; + fix.header.frame_id = frame_id_; + // set the timestamp + uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; + if (((m.valid & valid_time) == valid_time) && + (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) { + // Use NavPVT timestamp since it is valid + // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9 + // The ros time uses only unsigned values, so a negative nano seconds must be + // converted to a positive value + if (m.nano < 0) { + fix.header.stamp.sec = toUtcSeconds(m) - 1; + fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); + } + else { + fix.header.stamp.sec = toUtcSeconds(m); + fix.header.stamp.nsec = (uint32_t)(m.nano); + } + } else { + // Use ROS time since NavPVT timestamp is not valid + fix.header.stamp = ros::Time::now(); + } + // Set the LLA + fix.latitude = m.lat * 1e-7; // to deg + fix.longitude = m.lon * 1e-7; // to deg + fix.altitude = m.height * 1e-3; // to [m] + // Set the Fix status + bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; + if (fixOk && m.fix_type >= m.FIX_TYPE_2D) { + fix.status.status = fix.status.STATUS_FIX; + if (m.flags & m.CARRIER_PHASE_FIXED) { + fix.status.status = fix.status.STATUS_GBAS_FIX; + } + } else { + fix.status.status = fix.status.STATUS_NO_FIX; + } + // Set the service based on GNSS configuration + fix.status.service = fix_status_service_; + + // Set the position covariance + const double var_h = pow(m.h_acc / 1000.0, 2); // to [m^2] + const double var_v = pow(m.v_acc / 1000.0, 2); // to [m^2] + fix.position_covariance[0] = var_h; + fix.position_covariance[4] = var_h; + fix.position_covariance[8] = var_v; + fix.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_pub_.publish(fix); + + // + // Twist message + // + geometry_msgs::TwistWithCovarianceStamped velocity; + velocity.header.stamp = fix.header.stamp; + velocity.header.frame_id = frame_id_; + + // convert to XYZ linear velocity [m/s] in ENU + velocity.twist.twist.linear.x = m.vel_e * 1e-3; + velocity.twist.twist.linear.y = m.vel_n * 1e-3; + velocity.twist.twist.linear.z = -m.vel_d * 1e-3; + // Set the covariance + const double cov_speed = pow(m.s_acc * 1e-3, 2); + const int cols = 6; + velocity.twist.covariance[cols * 0 + 0] = cov_speed; + velocity.twist.covariance[cols * 1 + 1] = cov_speed; + velocity.twist.covariance[cols * 2 + 2] = cov_speed; + velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported + + vel_pub_.publish(velocity); + + // + // Update diagnostics + // + last_nav_pvt_ = m; + freq_diag_->diagnostic->tick(fix.header.stamp); + updater_->update(); + } + + protected: + + /** + * @brief Update the fix diagnostics from Nav PVT message. + */ + void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override { + // check the last message, convert to diagnostic + if (last_nav_pvt_.fix_type == + ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "Dead reckoning only"; + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_2D) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "2D fix"; + } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_3D) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "3D fix"; + } else if (last_nav_pvt_.fix_type == + ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "GPS and dead reckoning combined"; + } else if (last_nav_pvt_.fix_type == + ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Time only fix"; + } + + // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level + if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message += ", fix not ok"; + } + // Raise diagnostic level to error if no fix + if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { + stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.message = "No fix"; + } + + // append last fix position + stat.add("iTOW [ms]", last_nav_pvt_.i_tow); + stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7); + stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7); + stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3); + stat.add("Height above MSL [m]", last_nav_pvt_.h_msl * 1e-3); + stat.add("Horizontal Accuracy [m]", last_nav_pvt_.h_acc * 1e-3); + stat.add("Vertical Accuracy [m]", last_nav_pvt_.v_acc * 1e-3); + stat.add("# SVs used", (int)last_nav_pvt_.num_sv); + } + + //! The last received NavPVT message + NavPVT last_nav_pvt_; + // Whether or not to enable the given GNSS + //! Whether or not to enable GPS + bool enable_gps_; + //! Whether or not to enable GLONASS + bool enable_glonass_; + //! Whether or not to enable QZSS + bool enable_qzss_; + //! The QZSS Signal configuration, see CfgGNSS message + uint32_t qzss_sig_cfg_; + + ros::Publisher nav_pvt_pub_; + ros::Publisher fix_pub_; + ros::Publisher vel_pub_; + + std::string frame_id_; + std::shared_ptr freq_diag_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE7PLUS_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 91ecf1f3..0899dcd0 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include namespace ublox_node { From cfac6c279eeabf5d7c774567dd88ad8571804fff Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:18:41 +0000 Subject: [PATCH 073/152] Move UbloxFirmware7 to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 41 ---- .../include/ublox_gps/ublox_firmware7.hpp | 64 ++++++ ublox_gps/src/node.cpp | 181 +--------------- ublox_gps/src/ublox_firmware7.cpp | 197 ++++++++++++++++++ 5 files changed, 263 insertions(+), 222 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware7.hpp create mode 100644 ublox_gps/src/ublox_firmware7.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index f4be514a..371b6fa8 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index ab3e6ee9..a70e043a 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -294,47 +294,6 @@ class UbloxNode final { }; -/** - * @brief Implements functions for firmware version 7. - */ -class UbloxFirmware7 final : public UbloxFirmware7Plus { - public: - explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_svinfo_pub_ = node->advertise("navsvinfo", 1); - mon_hw_pub_ = node->advertise("monhw", 1); - } - - /** - * @brief Get the parameters specific to firmware version 7. - * - * @details Get the GNSS and NMEA settings. - */ - void getRosParams() override; - - /** - * @brief Configure GNSS individually. Only configures GLONASS. - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Subscribe to messages which are not generic to all firmware. - * - * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. - */ - void subscribe(std::shared_ptr gps) override; - - private: - //! Used to configure NMEA (if set_nmea_) - /*! - * Filled from ROS parameters - */ - ublox_msgs::CfgNMEA7 cfg_nmea_; - - ros::Publisher nav_svinfo_pub_; - ros::Publisher mon_hw_pub_; -}; - /** * @brief Implements functions for firmware version 8. */ diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp new file mode 100644 index 00000000..c5e934c2 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp @@ -0,0 +1,64 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE7_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE7_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 7. + */ +class UbloxFirmware7 final : public UbloxFirmware7Plus { + public: + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_svinfo_pub_ = node->advertise("navsvinfo", 1); + mon_hw_pub_ = node->advertise("monhw", 1); + } + + /** + * @brief Get the parameters specific to firmware version 7. + * + * @details Get the GNSS and NMEA settings. + */ + void getRosParams() override; + + /** + * @brief Configure GNSS individually. Only configures GLONASS. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to messages which are not generic to all firmware. + * + * @details Subscribe to NavPVT7 messages, RxmRAW, and RxmSFRB messages. + */ + void subscribe(std::shared_ptr gps) override; + + private: + //! Used to configure NMEA (if set_nmea_) + /*! + * Filled from ROS parameters + */ + ublox_msgs::CfgNMEA7 cfg_nmea_; + + ros::Publisher nav_svinfo_pub_; + ros::Publisher mon_hw_pub_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE7_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 0899dcd0..5c97225e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -781,186 +782,6 @@ void UbloxNode::shutdown() { } } -// -// Ublox Firmware Version 7 -// -void UbloxFirmware7::getRosParams() { - // - // GNSS configuration - // - // GNSS enable/disable - enable_gps_ = getRosBoolean(node_, "gnss/gps"); - enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); - enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); - - getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); - - if (enable_gps_ && !gnss_->isSupported("GPS")) { - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); - } - if (enable_glonass_ && !gnss_->isSupported("GLO")) { - ROS_WARN("gnss/glonass is true, but GLONASS is not %s", - "supported by this device"); - } - if (enable_qzss_ && !gnss_->isSupported("QZSS")) { - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - } - if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - } - - if (getRosBoolean(node_, "gnss/galileo")) { - ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); - } - if (getRosBoolean(node_, "gnss/beidou")) { - ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); - } - if (getRosBoolean(node_, "gnss/imes")) { - ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); - } - - // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; - - // - // NMEA Configuration - // - if (getRosBoolean(node_, "nmea/set")) { - bool compat, consider; - - if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - } - if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); - } - if (!node_->getParam("nmea/compat", compat)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - } - if (!node_->getParam("nmea/consider", consider)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - } - - // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; - // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - - getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); - } -} - -bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { - /** Configure the GNSS **/ - ublox_msgs::CfgGNSS cfgGNSSRead; - if (gps->poll(cfgGNSSRead)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); - ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); - } else { - throw std::runtime_error("Failed to read the GNSS config."); - } - - ublox_msgs::CfgGNSS cfgGNSSWrite; - cfgGNSSWrite.num_config_blocks = 1; // do services one by one - cfgGNSSWrite.num_trk_ch_hw = cfgGNSSRead.num_trk_ch_hw; - cfgGNSSWrite.num_trk_ch_use = cfgGNSSRead.num_trk_ch_use; - cfgGNSSWrite.msg_ver = 0; - - // configure GLONASS - if (gnss_->isSupported("GLO")) { - ublox_msgs::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_GLONASS; - block.res_trk_ch = block.RES_TRK_CH_GLONASS; - block.max_trk_ch = block.MAX_TRK_CH_GLONASS; - block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; - cfgGNSSWrite.blocks.push_back(block); - if (!gps->configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " GLONASS."); - } - } - - if (gnss_->isSupported("QZSS")) { - // configure QZSS - ublox_msgs::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_QZSS; - block.res_trk_ch = block.RES_TRK_CH_QZSS; - block.max_trk_ch = block.MAX_TRK_CH_QZSS; - block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; - cfgGNSSWrite.blocks[0] = block; - if (!gps->configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - ((enable_glonass_) ? "enable" : "disable") + - " QZSS."); - } - } - - if (gnss_->isSupported("SBAS")) { - // configure SBAS - ublox_msgs::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_SBAS; - block.res_trk_ch = block.RES_TRK_CH_SBAS; - block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = getRosBoolean(node_, "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; - cfgGNSSWrite.blocks[0] = block; - if (!gps->configure(cfgGNSSWrite)) { - throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(node_, "gnss/sbas") ? "enable" : "disable") + - " SBAS."); - } - } - - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { - throw std::runtime_error("Failed to configure NMEA"); - } - - return true; -} - -void UbloxFirmware7::subscribe(std::shared_ptr gps) { - // Subscribe to Nav PVT (always does so since fix information is published - // from this) - gps->subscribe(std::bind( - &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), - 1); - - // Subscribe to Nav SVINFO - if (getRosBoolean(node_, "publish/nav/svinfo")) { - gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, - kNavSvInfoSubscribeRate); - } - - // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, - 1); - } -} - // // Ublox Version 8 // diff --git a/ublox_gps/src/ublox_firmware7.cpp b/ublox_gps/src/ublox_firmware7.cpp new file mode 100644 index 00000000..c81807fc --- /dev/null +++ b/ublox_gps/src/ublox_firmware7.cpp @@ -0,0 +1,197 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// Ublox Firmware Version 7 +// +void UbloxFirmware7::getRosParams() { + // + // GNSS configuration + // + // GNSS enable/disable + enable_gps_ = getRosBoolean(node_, "gnss/gps"); + enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); + + getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + + if (enable_gps_ && !gnss_->isSupported("GPS")) { + ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); + } + if (enable_glonass_ && !gnss_->isSupported("GLO")) { + ROS_WARN("gnss/glonass is true, but GLONASS is not %s", + "supported by this device"); + } + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { + ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); + } + if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { + ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + } + + if (getRosBoolean(node_, "gnss/galileo")) { + ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); + } + if (getRosBoolean(node_, "gnss/beidou")) { + ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); + } + if (getRosBoolean(node_, "gnss/imes")) { + ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); + } + + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; + + // + // NMEA Configuration + // + if (getRosBoolean(node_, "nmea/set")) { + bool compat, consider; + + if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/version must be set"); + } + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/num_sv must be set"); + } + if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/sv_numbering must be set"); + } + if (!node_->getParam("nmea/compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); + } + if (!node_->getParam("nmea/consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/consider must be set"); + } + + // set flags + cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + // set gnssToFilter + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + + getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + } +} + +bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { + /** Configure the GNSS **/ + ublox_msgs::CfgGNSS cfgGNSSRead; + if (gps->poll(cfgGNSSRead)) { + ROS_DEBUG("Read GNSS config."); + ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); + ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); + } else { + throw std::runtime_error("Failed to read the GNSS config."); + } + + ublox_msgs::CfgGNSS cfgGNSSWrite; + cfgGNSSWrite.num_config_blocks = 1; // do services one by one + cfgGNSSWrite.num_trk_ch_hw = cfgGNSSRead.num_trk_ch_hw; + cfgGNSSWrite.num_trk_ch_use = cfgGNSSRead.num_trk_ch_use; + cfgGNSSWrite.msg_ver = 0; + + // configure GLONASS + if (gnss_->isSupported("GLO")) { + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_GLONASS; + block.res_trk_ch = block.RES_TRK_CH_GLONASS; + block.max_trk_ch = block.MAX_TRK_CH_GLONASS; + block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; + cfgGNSSWrite.blocks.push_back(block); + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + ((enable_glonass_) ? "enable" : "disable") + + " GLONASS."); + } + } + + if (gnss_->isSupported("QZSS")) { + // configure QZSS + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_QZSS; + block.res_trk_ch = block.RES_TRK_CH_QZSS; + block.max_trk_ch = block.MAX_TRK_CH_QZSS; + block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; + cfgGNSSWrite.blocks[0] = block; + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + ((enable_glonass_) ? "enable" : "disable") + + " QZSS."); + } + } + + if (gnss_->isSupported("SBAS")) { + // configure SBAS + ublox_msgs::CfgGNSSBlock block; + block.gnss_id = block.GNSS_ID_SBAS; + block.res_trk_ch = block.RES_TRK_CH_SBAS; + block.max_trk_ch = block.MAX_TRK_CH_SBAS; + block.flags = getRosBoolean(node_, "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; + cfgGNSSWrite.blocks[0] = block; + if (!gps->configure(cfgGNSSWrite)) { + throw std::runtime_error(std::string("Failed to ") + + (getRosBoolean(node_, "gnss/sbas") ? "enable" : "disable") + + " SBAS."); + } + } + + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware7::subscribe(std::shared_ptr gps) { + // Subscribe to Nav PVT (always does so since fix information is published + // from this) + gps->subscribe(std::bind( + &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), + 1); + + // Subscribe to Nav SVINFO + if (getRosBoolean(node_, "publish/nav/svinfo")) { + gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish/mon/hw")) { + gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + 1); + } +} + +} // namespace ublox_node From f96d55a6669cb2c7d8ffbf86f773e0303fde82f0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:26:00 +0000 Subject: [PATCH 074/152] Move UbloxFirmware8 class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 57 +--- .../include/ublox_gps/ublox_firmware8.hpp | 79 ++++++ ublox_gps/src/node.cpp | 236 +--------------- ublox_gps/src/ublox_firmware8.cpp | 256 ++++++++++++++++++ 5 files changed, 338 insertions(+), 292 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware8.hpp create mode 100644 ublox_gps/src/ublox_firmware8.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 371b6fa8..270313f4 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index a70e043a..ab2c72f1 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -59,6 +59,7 @@ #include #include #include +#include #include // This file also declares UbloxNode which implements ComponentInterface and is @@ -294,62 +295,6 @@ class UbloxNode final { }; -/** - * @brief Implements functions for firmware version 8. - */ -class UbloxFirmware8 : public UbloxFirmware7Plus { - public: - explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_sat_pub_ = node->advertise("navstate", 1); - mon_hw_pub_ = node->advertise("monhw", 1); - rxm_rtcm_pub_ = node->advertise("rxmrtcm", 1); - } - - /** - * @brief Get the ROS parameters specific to firmware version 8. - * - * @details Get the GNSS, NMEA, and UPD settings. - */ - void getRosParams() override; - - /** - * @brief Configure settings specific to firmware 8 based on ROS parameters. - * - * @details Configure GNSS, if it is different from current settings. - * Configure the NMEA if desired by the user. It also may clear the - * flash memory based on the ROS parameters. - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Subscribe to u-blox messages which are not generic to all firmware - * versions. - * - * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based - * on user settings. - */ - void subscribe(std::shared_ptr gps) override; - - private: - // Set from ROS parameters - //! Whether or not to enable the Galileo GNSS - bool enable_galileo_; - //! Whether or not to enable the BeiDuo GNSS - bool enable_beidou_; - //! Whether or not to enable the IMES GNSS - bool enable_imes_; - //! Desired NMEA configuration. - ublox_msgs::CfgNMEA cfg_nmea_; - //! Whether to clear the flash memory during configuration - bool clear_bbr_; - bool save_on_shutdown_; - - ros::Publisher nav_sat_pub_; - ros::Publisher mon_hw_pub_; - ros::Publisher rxm_rtcm_pub_; -}; - /** * @brief Implements functions for firmware version 9. * For now it simply re-uses the firmware version 8 class diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp new file mode 100644 index 00000000..f0ba0351 --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -0,0 +1,79 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE8_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE8_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 8. + */ +class UbloxFirmware8 : public UbloxFirmware7Plus { + public: + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_sat_pub_ = node->advertise("navstate", 1); + mon_hw_pub_ = node->advertise("monhw", 1); + rxm_rtcm_pub_ = node->advertise("rxmrtcm", 1); + } + + /** + * @brief Get the ROS parameters specific to firmware version 8. + * + * @details Get the GNSS, NMEA, and UPD settings. + */ + void getRosParams() override; + + /** + * @brief Configure settings specific to firmware 8 based on ROS parameters. + * + * @details Configure GNSS, if it is different from current settings. + * Configure the NMEA if desired by the user. It also may clear the + * flash memory based on the ROS parameters. + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Subscribe to u-blox messages which are not generic to all firmware + * versions. + * + * @details Subscribe to NavPVT, NavSAT, MonHW, and RxmRTCM messages based + * on user settings. + */ + void subscribe(std::shared_ptr gps) override; + + private: + // Set from ROS parameters + //! Whether or not to enable the Galileo GNSS + bool enable_galileo_; + //! Whether or not to enable the BeiDuo GNSS + bool enable_beidou_; + //! Whether or not to enable the IMES GNSS + bool enable_imes_; + //! Desired NMEA configuration. + ublox_msgs::CfgNMEA cfg_nmea_; + //! Whether to clear the flash memory during configuration + bool clear_bbr_; + bool save_on_shutdown_; + + ros::Publisher nav_sat_pub_; + ros::Publisher mon_hw_pub_; + ros::Publisher rxm_rtcm_pub_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE8_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 5c97225e..b30e0e7f 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include namespace ublox_node { @@ -782,241 +783,6 @@ void UbloxNode::shutdown() { } } -// -// Ublox Version 8 -// -void UbloxFirmware8::getRosParams() { - // UPD SOS configuration - clear_bbr_ = getRosBoolean(node_, "clear_bbr"); - save_on_shutdown_ = getRosBoolean(node_, "save_on_shutdown"); - - // GNSS enable/disable - enable_gps_ = getRosBoolean(node_, "gnss/gps"); - enable_galileo_ = getRosBoolean(node_, "gnss/galileo"); - enable_beidou_ = getRosBoolean(node_, "gnss/beidou"); - enable_imes_ = getRosBoolean(node_, "gnss/imes"); - enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); - enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); - - // QZSS Signal Configuration - getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); - - if (enable_gps_ && !gnss_->isSupported("GPS")) { - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", - "this device"); - } - if (enable_glonass_ && !gnss_->isSupported("GLO")) { - ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", - "this device"); - } - if (enable_galileo_ && !gnss_->isSupported("GAL")) { - ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", - "by this device"); - } - if (enable_beidou_ && !gnss_->isSupported("BDS")) { - ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", - "by this device"); - } - if (enable_imes_ && !gnss_->isSupported("IMES")) { - ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", - "this device"); - } - if (enable_qzss_ && !gnss_->isSupported("QZSS")) { - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); - } - if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); - } - - // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; - - // - // NMEA Configuration - // - if (getRosBoolean(node_, "nmea/set")) { - bool compat, consider; - cfg_nmea_.version = cfg_nmea_.VERSION; // message version - - // Verify that parameters are set - if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); - } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); - } - if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); - } - if (!node_->getParam("nmea/compat", compat)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); - } - if (!node_->getParam("nmea/consider", consider)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); - } - - // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; - // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; - // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - - getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); - - std::vector bds_talker_id; - getRosUint(node_, "nmea/bds_talker_id", bds_talker_id); - cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; - cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; - } -} - -bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { - if (clear_bbr_) { - // clear flash memory - if (!gps->clearBbr()) { - ROS_ERROR("u-blox failed to clear flash memory"); - } - } - - gps->setSaveOnShutdown(save_on_shutdown_); - - // - // Configure the GNSS, only if the configuration is different - // - // First, get the current GNSS configuration - ublox_msgs::CfgGNSS cfg_gnss; - if (gps->poll(cfg_gnss)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); - ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); - } else { - throw std::runtime_error("Failed to read the GNSS config."); - } - - // Then, check the configuration for each GNSS. If it is different, change it. - bool correct = true; - for (int i = 0; i < cfg_gnss.blocks.size(); i++) { - ublox_msgs::CfgGNSSBlock block = cfg_gnss.blocks[i]; - if (block.gnss_id == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; - ROS_DEBUG("GPS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_SBAS - && getRosBoolean(node_, "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss/sbas"); - ROS_DEBUG("SBAS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; - ROS_DEBUG("Galileo GNSS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; - ROS_DEBUG("BeiDou Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnss_id == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) - || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { - ROS_DEBUG("QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, - enable_qzss_); - correct = false; - ROS_DEBUG("QZSS Configuration: %u", block.flags); - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; - ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); - if (enable_qzss_) { - // Only change sig cfg if enabling - cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; - } - } else if (block.gnss_id == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { - correct = false; - cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; - ROS_DEBUG("GLONASS Configuration is different"); - } - } - - // If the GNSS is already configured correctly, do not re-configure GNSS - // since this requires a cold reset - if (correct) { - ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); - } else if (!gps->configGnss(cfg_gnss, std::chrono::seconds(15))) { - throw std::runtime_error(std::string("Failed to cold reset device ") + - "after configuring GNSS"); - } - - // - // NMEA config - // - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { - throw std::runtime_error("Failed to configure NMEA"); - } - - return true; -} - -void UbloxFirmware8::subscribe(std::shared_ptr gps) { - // Subscribe to Nav PVT - gps->subscribe( - std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); - - // Subscribe to Nav SAT messages - if (getRosBoolean(node_, "publish/nav/sat")) { - gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, - kNavSvInfoSubscribeRate); - } - - // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, - 1); - } - - // Subscribe to RTCM messages - if (getRosBoolean(node_, "publish/rxm/rtcm")) { - gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, - 1); - } -} - UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) { diff --git a/ublox_gps/src/ublox_firmware8.cpp b/ublox_gps/src/ublox_firmware8.cpp new file mode 100644 index 00000000..b91ad0ed --- /dev/null +++ b/ublox_gps/src/ublox_firmware8.cpp @@ -0,0 +1,256 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace ublox_node { + +// +// Ublox Version 8 +// +void UbloxFirmware8::getRosParams() { + // UPD SOS configuration + clear_bbr_ = getRosBoolean(node_, "clear_bbr"); + save_on_shutdown_ = getRosBoolean(node_, "save_on_shutdown"); + + // GNSS enable/disable + enable_gps_ = getRosBoolean(node_, "gnss/gps"); + enable_galileo_ = getRosBoolean(node_, "gnss/galileo"); + enable_beidou_ = getRosBoolean(node_, "gnss/beidou"); + enable_imes_ = getRosBoolean(node_, "gnss/imes"); + enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); + + // QZSS Signal Configuration + getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + + if (enable_gps_ && !gnss_->isSupported("GPS")) { + ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", + "this device"); + } + if (enable_glonass_ && !gnss_->isSupported("GLO")) { + ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", + "this device"); + } + if (enable_galileo_ && !gnss_->isSupported("GAL")) { + ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", + "by this device"); + } + if (enable_beidou_ && !gnss_->isSupported("BDS")) { + ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", + "by this device"); + } + if (enable_imes_ && !gnss_->isSupported("IMES")) { + ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", + "this device"); + } + if (enable_qzss_ && !gnss_->isSupported("QZSS")) { + ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); + } + if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { + ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + } + + // Fix Service type, used when publishing fix status messages + fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; + + // + // NMEA Configuration + // + if (getRosBoolean(node_, "nmea/set")) { + bool compat, consider; + cfg_nmea_.version = cfg_nmea_.VERSION; // message version + + // Verify that parameters are set + if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/version must be set"); + } + if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/num_sv must be set"); + } + if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/sv_numbering must be set"); + } + if (!node_->getParam("nmea/compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/compat must be set"); + } + if (!node_->getParam("nmea/consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + + "true, therefore nmea/consider must be set"); + } + + // set flags + cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + // set filter + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + // set gnssToFilter + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + + getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + + std::vector bds_talker_id; + getRosUint(node_, "nmea/bds_talker_id", bds_talker_id); + cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; + cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; + } +} + +bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { + if (clear_bbr_) { + // clear flash memory + if (!gps->clearBbr()) { + ROS_ERROR("u-blox failed to clear flash memory"); + } + } + + gps->setSaveOnShutdown(save_on_shutdown_); + + // + // Configure the GNSS, only if the configuration is different + // + // First, get the current GNSS configuration + ublox_msgs::CfgGNSS cfg_gnss; + if (gps->poll(cfg_gnss)) { + ROS_DEBUG("Read GNSS config."); + ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); + } else { + throw std::runtime_error("Failed to read the GNSS config."); + } + + // Then, check the configuration for each GNSS. If it is different, change it. + bool correct = true; + for (int i = 0; i < cfg_gnss.blocks.size(); i++) { + ublox_msgs::CfgGNSSBlock block = cfg_gnss.blocks[i]; + if (block.gnss_id == block.GNSS_ID_GPS + && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; + ROS_DEBUG("GPS Configuration is different"); + } else if (block.gnss_id == block.GNSS_ID_SBAS + && getRosBoolean(node_, "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss/sbas"); + ROS_DEBUG("SBAS Configuration is different"); + } else if (block.gnss_id == block.GNSS_ID_GALILEO + && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; + ROS_DEBUG("Galileo GNSS Configuration is different"); + } else if (block.gnss_id == block.GNSS_ID_BEIDOU + && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; + ROS_DEBUG("BeiDou Configuration is different"); + } else if (block.gnss_id == block.GNSS_ID_IMES + && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; + } else if (block.gnss_id == block.GNSS_ID_QZSS + && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) + || (enable_qzss_ + && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { + ROS_DEBUG("QZSS Configuration is different %u, %u", + block.flags & block.FLAGS_ENABLE, + enable_qzss_); + correct = false; + ROS_DEBUG("QZSS Configuration: %u", block.flags); + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; + ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); + if (enable_qzss_) { + // Only change sig cfg if enabling + cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; + } + } else if (block.gnss_id == block.GNSS_ID_GLONASS + && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { + correct = false; + cfg_gnss.blocks[i].flags = + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; + ROS_DEBUG("GLONASS Configuration is different"); + } + } + + // If the GNSS is already configured correctly, do not re-configure GNSS + // since this requires a cold reset + if (correct) { + ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); + } else if (!gps->configGnss(cfg_gnss, std::chrono::seconds(15))) { + throw std::runtime_error(std::string("Failed to cold reset device ") + + "after configuring GNSS"); + } + + // + // NMEA config + // + if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +void UbloxFirmware8::subscribe(std::shared_ptr gps) { + // Subscribe to Nav PVT + gps->subscribe( + std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); + + // Subscribe to Nav SAT messages + if (getRosBoolean(node_, "publish/nav/sat")) { + gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, + kNavSvInfoSubscribeRate); + } + + // Subscribe to Mon HW + if (getRosBoolean(node_, "publish/mon/hw")) { + gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + 1); + } + + // Subscribe to RTCM messages + if (getRosBoolean(node_, "publish/rxm/rtcm")) { + gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, + 1); + } +} + +} // namespace ublox_node From 41472c09d4910715148f573e9090379f2217f434 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:30:29 +0000 Subject: [PATCH 075/152] Move UbloxFirmware9 class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 10 ------- .../include/ublox_gps/ublox_firmware9.hpp | 28 +++++++++++++++++++ ublox_gps/src/node.cpp | 6 +--- ublox_gps/src/ublox_firmware9.cpp | 19 +++++++++++++ 5 files changed, 49 insertions(+), 16 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/ublox_firmware9.hpp create mode 100644 ublox_gps/src/ublox_firmware9.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 270313f4..967d59a5 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index ab2c72f1..94f1552a 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -295,16 +295,6 @@ class UbloxNode final { }; -/** - * @brief Implements functions for firmware version 9. - * For now it simply re-uses the firmware version 8 class - * but allows for future expansion of functionality - */ -class UbloxFirmware9 final : public UbloxFirmware8 { -public: - explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); -}; - /** * @brief Implements functions for High Precision GNSS Reference station * devices. diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp new file mode 100644 index 00000000..d9b2214b --- /dev/null +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -0,0 +1,28 @@ +#ifndef UBLOX_GPS_UBLOX_FIRMWARE9_HPP +#define UBLOX_GPS_UBLOX_FIRMWARE9_HPP + +#include +#include + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for firmware version 9. + * For now it simply re-uses the firmware version 8 class + * but allows for future expansion of functionality + */ +class UbloxFirmware9 final : public UbloxFirmware8 { +public: + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_UBLOX_FIRMWARE9_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index b30e0e7f..dc433c6a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include namespace ublox_node { @@ -783,11 +784,6 @@ void UbloxNode::shutdown() { } } -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) -{ -} - // // u-blox High Precision GNSS Reference Station // diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp new file mode 100644 index 00000000..fddc9e24 --- /dev/null +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -0,0 +1,19 @@ +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace ublox_node { + +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) +{ +} + +} // namespace ublox_node From 7e88539846a15dbd84d4a1c1557029bb3f127e4a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:36:35 +0000 Subject: [PATCH 076/152] Move HpgRefProduct class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- .../include/ublox_gps/hpg_ref_product.hpp | 145 +++++++++++ ublox_gps/include/ublox_gps/node.hpp | 126 +--------- ublox_gps/src/hpg_ref_product.cpp | 235 ++++++++++++++++++ ublox_gps/src/node.cpp | 216 +--------------- 5 files changed, 383 insertions(+), 341 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/hpg_ref_product.hpp create mode 100644 ublox_gps/src/hpg_ref_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 967d59a5..1f96956a 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp new file mode 100644 index 00000000..81618781 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp @@ -0,0 +1,145 @@ +#ifndef UBLOX_GPS_HPG_REF_PRODUCT_HPP +#define UBLOX_GPS_HPG_REF_PRODUCT_HPP + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief Implements functions for High Precision GNSS Reference station + * devices. + */ +class HpgRefProduct: public virtual ComponentInterface { + public: + //! Default measurement period for HPG devices + constexpr static uint16_t kDefaultMeasPeriod = 250; + + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); + + /** + * @brief Get the ROS parameters specific to the Reference Station + * configuration. + * + * @details Get the TMODE3 settings, the parameters it gets depends on the + * tmode3 parameter. For example, it will get survey-in parameters if the + * tmode3 parameter is set to survey in or it will get the fixed parameters if + * it is set to fixed. + */ + void getRosParams() override; + + /** + * @brief Configure the u-blox Reference Station settings. + * + * @details Configure the TMODE3 settings and sets the internal state based + * on the TMODE3 status. If the TMODE3 is set to fixed, it will configure + * the RTCM messages. + * @return true if configured correctly, false otherwise + */ + bool configureUblox(std::shared_ptr gps) override; + + /** + * @brief Add diagnostic updaters for the TMODE3 status. + */ + void initializeRosDiagnostics() override; + + /** + * @brief Subscribe to u-blox Reference Station messages. + * + * @details Subscribe to NavSVIN messages based on user parameters. + */ + void subscribe(std::shared_ptr gps) override; + + /** + * @brief Update the last received NavSVIN message and call diagnostic updater + * + * @details When the survey in finishes, it changes the measurement & + * navigation rate to the user configured values and enables the user + * configured RTCM messages. Publish received Nav SVIN messages if enabled. + * @param m the message to process + */ + void callbackNavSvIn(const ublox_msgs::NavSVIN& m); + + protected: + /** + * @brief Update the TMODE3 diagnostics. + * + * @details Updates the status of the survey-in if in survey-in mode or the + * RTCM messages if in time mode. + */ + void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + + /** + * @brief Set the device mode to time mode (internal state variable). + * + * @details Configure the RTCM messages and measurement and navigation rate. + */ + bool setTimeMode(std::shared_ptr gps); + + //! The last received Nav SVIN message + ublox_msgs::NavSVIN last_nav_svin_; + + //! TMODE3 to set, such as disabled, survey-in, fixed + uint8_t tmode3_; + + // TMODE3 = Fixed mode settings + //! True if coordinates are in LLA, false if ECEF + /*! Used only for fixed mode */ + bool lla_flag_; + //! Antenna Reference Point Position [m] or [deg] + /*! Used only for fixed mode */ + std::vector arp_position_; + //! Antenna Reference Point Position High Precision [0.1 mm] or [deg * 1e-9] + /*! Used only for fixed mode */ + std::vector arp_position_hp_; + //! Fixed Position Accuracy [m] + /*! Used only for fixed mode */ + float fixed_pos_acc_; + + // Settings for TMODE3 = Survey-in + //! Whether to always reset the survey-in during configuration. + /*! + * If false, it only resets survey-in if there's no fix and TMODE3 is + * disabled before configuration. + * This variable is used only if TMODE3 is set to survey-in. + */ + bool svin_reset_; + //! Measurement period used during Survey-In [s] + /*! This variable is used only if TMODE3 is set to survey-in. */ + uint32_t sv_in_min_dur_; + //! Survey in accuracy limit [m] + /*! This variable is used only if TMODE3 is set to survey-in. */ + float sv_in_acc_lim_; + + //! Status of device time mode + enum { + INIT, //!< Initialization mode (before configuration) + FIXED, //!< Fixed mode (should switch to time mode almost immediately) + DISABLED, //!< Time mode disabled + SURVEY_IN, //!< Survey-In mode + TIME //!< Time mode, after survey-in or after configuring fixed mode + } mode_; + + ros::Publisher navsvin_pub_; + + uint16_t nav_rate_; + uint16_t meas_rate_; + std::shared_ptr updater_; + + std::vector rtcms_; + std::shared_ptr gps_; + ros::NodeHandle* node_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HPG_REF_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 94f1552a..d6fbe501 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -295,131 +296,6 @@ class UbloxNode final { }; -/** - * @brief Implements functions for High Precision GNSS Reference station - * devices. - */ -class HpgRefProduct: public virtual ComponentInterface { - public: - //! Default measurement period for HPG devices - constexpr static uint16_t kDefaultMeasPeriod = 250; - - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); - - /** - * @brief Get the ROS parameters specific to the Reference Station - * configuration. - * - * @details Get the TMODE3 settings, the parameters it gets depends on the - * tmode3 parameter. For example, it will get survey-in parameters if the - * tmode3 parameter is set to survey in or it will get the fixed parameters if - * it is set to fixed. - */ - void getRosParams() override; - - /** - * @brief Configure the u-blox Reference Station settings. - * - * @details Configure the TMODE3 settings and sets the internal state based - * on the TMODE3 status. If the TMODE3 is set to fixed, it will configure - * the RTCM messages. - * @return true if configured correctly, false otherwise - */ - bool configureUblox(std::shared_ptr gps) override; - - /** - * @brief Add diagnostic updaters for the TMODE3 status. - */ - void initializeRosDiagnostics() override; - - /** - * @brief Subscribe to u-blox Reference Station messages. - * - * @details Subscribe to NavSVIN messages based on user parameters. - */ - void subscribe(std::shared_ptr gps) override; - - /** - * @brief Update the last received NavSVIN message and call diagnostic updater - * - * @details When the survey in finishes, it changes the measurement & - * navigation rate to the user configured values and enables the user - * configured RTCM messages. Publish received Nav SVIN messages if enabled. - * @param m the message to process - */ - void callbackNavSvIn(const ublox_msgs::NavSVIN& m); - - protected: - /** - * @brief Update the TMODE3 diagnostics. - * - * @details Updates the status of the survey-in if in survey-in mode or the - * RTCM messages if in time mode. - */ - void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - - /** - * @brief Set the device mode to time mode (internal state variable). - * - * @details Configure the RTCM messages and measurement and navigation rate. - */ - bool setTimeMode(std::shared_ptr gps); - - //! The last received Nav SVIN message - ublox_msgs::NavSVIN last_nav_svin_; - - //! TMODE3 to set, such as disabled, survey-in, fixed - uint8_t tmode3_; - - // TMODE3 = Fixed mode settings - //! True if coordinates are in LLA, false if ECEF - /*! Used only for fixed mode */ - bool lla_flag_; - //! Antenna Reference Point Position [m] or [deg] - /*! Used only for fixed mode */ - std::vector arp_position_; - //! Antenna Reference Point Position High Precision [0.1 mm] or [deg * 1e-9] - /*! Used only for fixed mode */ - std::vector arp_position_hp_; - //! Fixed Position Accuracy [m] - /*! Used only for fixed mode */ - float fixed_pos_acc_; - - // Settings for TMODE3 = Survey-in - //! Whether to always reset the survey-in during configuration. - /*! - * If false, it only resets survey-in if there's no fix and TMODE3 is - * disabled before configuration. - * This variable is used only if TMODE3 is set to survey-in. - */ - bool svin_reset_; - //! Measurement period used during Survey-In [s] - /*! This variable is used only if TMODE3 is set to survey-in. */ - uint32_t sv_in_min_dur_; - //! Survey in accuracy limit [m] - /*! This variable is used only if TMODE3 is set to survey-in. */ - float sv_in_acc_lim_; - - //! Status of device time mode - enum { - INIT, //!< Initialization mode (before configuration) - FIXED, //!< Fixed mode (should switch to time mode almost immediately) - DISABLED, //!< Time mode disabled - SURVEY_IN, //!< Survey-In mode - TIME //!< Time mode, after survey-in or after configuring fixed mode - } mode_; - - ros::Publisher navsvin_pub_; - - uint16_t nav_rate_; - uint16_t meas_rate_; - std::shared_ptr updater_; - - std::vector rtcms_; - std::shared_ptr gps_; - ros::NodeHandle* node_; -}; - class HpPosRecProduct final : public virtual HpgRefProduct { public: explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp new file mode 100644 index 00000000..a7bd8567 --- /dev/null +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -0,0 +1,235 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace ublox_node { + +// +// u-blox High Precision GNSS Reference Station +// + +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) + : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) +{ + navsvin_pub_ = + node_->advertise("navsvin", 1); +} + +void HpgRefProduct::getRosParams() { + if (getRosBoolean(node_, "config_on_startup")) { + if (nav_rate_ * meas_rate_ != 1000) { + ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); + } + + if (!getRosUint(node_, "tmode3", tmode3_)) { + throw std::runtime_error("Invalid settings: TMODE3 must be set"); + } + + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!node_->getParam("arp/position", arp_position_)) { + throw std::runtime_error(std::string("Invalid settings: arp/position ") + + "must be set if TMODE3 is fixed"); + } + if (!getRosInt(node_, "arp/position_hp", arp_position_hp_)) { + throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + + "must be set if TMODE3 is fixed"); + } + if (!node_->getParam("arp/acc", fixed_pos_acc_)) { + throw std::runtime_error(std::string("Invalid settings: arp/acc ") + + "must be set if TMODE3 is fixed"); + } + if (!node_->getParam("arp/lla_flag", lla_flag_)) { + ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", + "in ECEF"); + lla_flag_ = false; + } + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + svin_reset_ = getRosBoolean(node_, "sv_in/reset"); + if (!getRosUint(node_, "sv_in/min_dur", sv_in_min_dur_)) { + throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + + "must be set if TMODE3 is survey-in"); + } + if (!node_->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { + throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + + "must be set if TMODE3 is survey-in"); + } + } else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + + " flag constants for possible values."); + } + } +} + +bool HpgRefProduct::configureUblox(std::shared_ptr gps) { + // Configure TMODE3 + if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + if (!gps->disableTmode3()) { + throw std::runtime_error("Failed to disable TMODE3."); + } + mode_ = DISABLED; + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!gps->configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, + fixed_pos_acc_)) { + throw std::runtime_error("Failed to set TMODE3 to fixed."); + } + if (!gps->configRtcm(rtcms_)) { + throw std::runtime_error("Failed to set RTCM rates"); + } + mode_ = FIXED; + } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + if (!svin_reset_) { + ublox_msgs::NavSVIN nav_svin; + if (!gps->poll(nav_svin)) { + throw std::runtime_error(std::string("Failed to poll NavSVIN while") + + " configuring survey-in"); + } + // Don't reset survey-in if it's already active + if (nav_svin.active) { + mode_ = SURVEY_IN; + return true; + } + // Don't reset survey-in if it already has a valid value + if (nav_svin.valid) { + setTimeMode(gps); + return true; + } + ublox_msgs::NavPVT nav_pvt; + if (!gps->poll(nav_pvt)) { + throw std::runtime_error(std::string("Failed to poll NavPVT while") + + " configuring survey-in"); + } + // Don't reset survey in if in time mode with a good fix + if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY + && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { + setTimeMode(gps); + return true; + } + } + // Reset the Survey In + // For Survey in, meas rate must be at least 1 Hz + uint16_t meas_rate_temp = meas_rate_ < 1000 ? meas_rate_ : 1000; // [ms] + // If measurement period isn't a factor of 1000, set to default + if (1000 % meas_rate_temp != 0) { + meas_rate_temp = kDefaultMeasPeriod; + } + // Set nav rate to 1 Hz during survey in + if (!gps->configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { + throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + + "before setting TMODE3 to survey-in."); + } + // As recommended in the documentation, first disable, then set to survey in + if (!gps->disableTmode3()) { + ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); + } else { + mode_ = DISABLED; + } + // Set to Survey in mode + if (!gps->configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { + throw std::runtime_error("Failed to set TMODE3 to survey-in."); + } + mode_ = SURVEY_IN; + } + return true; +} + +void HpgRefProduct::subscribe(std::shared_ptr gps) { + // Subscribe to Nav Survey-In + // Save off the gps pointer so we can use it in the callback later. + gps_ = gps; + gps->subscribe(std::bind( + &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); +} + +void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { + if (getRosBoolean(node_, "publish/nav/svin")) { + navsvin_pub_.publish(m); + } + + last_nav_svin_ = m; + + if (!m.active && m.valid && mode_ == SURVEY_IN) { + setTimeMode(gps_); + } + + updater_->update(); +} + +bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { + ROS_INFO("Setting mode (internal state) to Time Mode"); + mode_ = TIME; + + // Set the Measurement & nav rate to user config + // (survey-in sets nav_rate to 1 Hz regardless of user setting) + if (!gps->configRate(meas_rate_, nav_rate_)) { + ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate_, + "navigation rate to ", nav_rate_); + } + // Enable the RTCM out messages + if (!gps->configRtcm(rtcms_)) { + ROS_ERROR("Failed to configure RTCM IDs"); + return false; + } + return true; +} + +void HpgRefProduct::initializeRosDiagnostics() { + updater_->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); + updater_->force_update(); +} + +void HpgRefProduct::tmode3Diagnostics( + diagnostic_updater::DiagnosticStatusWrapper& stat) { + if (mode_ == INIT) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "Not configured"; + } else if (mode_ == DISABLED){ + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Disabled"; + } else if (mode_ == SURVEY_IN) { + if (!last_nav_svin_.active && !last_nav_svin_.valid) { + stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.message = "Survey-In inactive and invalid"; + } else if (last_nav_svin_.active && !last_nav_svin_.valid) { + stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.message = "Survey-In active but invalid"; + } else if (!last_nav_svin_.active && last_nav_svin_.valid) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Survey-In complete"; + } else if (last_nav_svin_.active && last_nav_svin_.valid) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Survey-In active and valid"; + } + + stat.add("iTOW [ms]", last_nav_svin_.i_tow); + stat.add("Duration [s]", last_nav_svin_.dur); + stat.add("# observations", last_nav_svin_.obs); + stat.add("Mean X [m]", last_nav_svin_.mean_x * 1e-2); + stat.add("Mean Y [m]", last_nav_svin_.mean_y * 1e-2); + stat.add("Mean Z [m]", last_nav_svin_.mean_z * 1e-2); + stat.add("Mean X HP [m]", last_nav_svin_.mean_xhp * 1e-4); + stat.add("Mean Y HP [m]", last_nav_svin_.mean_yhp * 1e-4); + stat.add("Mean Z HP [m]", last_nav_svin_.mean_zhp * 1e-4); + stat.add("Mean Accuracy [m]", last_nav_svin_.mean_acc * 1e-4); + } else if (mode_ == FIXED) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Fixed Position"; + } else if (mode_ == TIME) { + stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.message = "Time"; + } +} + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index dc433c6a..c2ca52dd 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -784,221 +785,6 @@ void UbloxNode::shutdown() { } } -// -// u-blox High Precision GNSS Reference Station -// - -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) - : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) -{ - navsvin_pub_ = - node_->advertise("navsvin", 1); -} - -void HpgRefProduct::getRosParams() { - if (getRosBoolean(node_, "config_on_startup")) { - if (nav_rate_ * meas_rate_ != 1000) { - ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); - } - - if (!getRosUint(node_, "tmode3", tmode3_)) { - throw std::runtime_error("Invalid settings: TMODE3 must be set"); - } - - if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if (!node_->getParam("arp/position", arp_position_)) { - throw std::runtime_error(std::string("Invalid settings: arp/position ") - + "must be set if TMODE3 is fixed"); - } - if (!getRosInt(node_, "arp/position_hp", arp_position_hp_)) { - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") - + "must be set if TMODE3 is fixed"); - } - if (!node_->getParam("arp/acc", fixed_pos_acc_)) { - throw std::runtime_error(std::string("Invalid settings: arp/acc ") - + "must be set if TMODE3 is fixed"); - } - if (!node_->getParam("arp/lla_flag", lla_flag_)) { - ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", - "in ECEF"); - lla_flag_ = false; - } - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - svin_reset_ = getRosBoolean(node_, "sv_in/reset"); - if (!getRosUint(node_, "sv_in/min_dur", sv_in_min_dur_)) { - throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") - + "must be set if TMODE3 is survey-in"); - } - if (!node_->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { - throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") - + "must be set if TMODE3 is survey-in"); - } - } else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") - + " flag constants for possible values."); - } - } -} - -bool HpgRefProduct::configureUblox(std::shared_ptr gps) { - // Configure TMODE3 - if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { - if (!gps->disableTmode3()) { - throw std::runtime_error("Failed to disable TMODE3."); - } - mode_ = DISABLED; - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if (!gps->configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, - fixed_pos_acc_)) { - throw std::runtime_error("Failed to set TMODE3 to fixed."); - } - if (!gps->configRtcm(rtcms_)) { - throw std::runtime_error("Failed to set RTCM rates"); - } - mode_ = FIXED; - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - if (!svin_reset_) { - ublox_msgs::NavSVIN nav_svin; - if (!gps->poll(nav_svin)) { - throw std::runtime_error(std::string("Failed to poll NavSVIN while") + - " configuring survey-in"); - } - // Don't reset survey-in if it's already active - if (nav_svin.active) { - mode_ = SURVEY_IN; - return true; - } - // Don't reset survey-in if it already has a valid value - if (nav_svin.valid) { - setTimeMode(gps); - return true; - } - ublox_msgs::NavPVT nav_pvt; - if (!gps->poll(nav_pvt)) { - throw std::runtime_error(std::string("Failed to poll NavPVT while") + - " configuring survey-in"); - } - // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { - setTimeMode(gps); - return true; - } - } - // Reset the Survey In - // For Survey in, meas rate must be at least 1 Hz - uint16_t meas_rate_temp = meas_rate_ < 1000 ? meas_rate_ : 1000; // [ms] - // If measurement period isn't a factor of 1000, set to default - if (1000 % meas_rate_temp != 0) { - meas_rate_temp = kDefaultMeasPeriod; - } - // Set nav rate to 1 Hz during survey in - if (!gps->configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { - throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + - "before setting TMODE3 to survey-in."); - } - // As recommended in the documentation, first disable, then set to survey in - if (!gps->disableTmode3()) { - ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); - } else { - mode_ = DISABLED; - } - // Set to Survey in mode - if (!gps->configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_)) { - throw std::runtime_error("Failed to set TMODE3 to survey-in."); - } - mode_ = SURVEY_IN; - } - return true; -} - -void HpgRefProduct::subscribe(std::shared_ptr gps) { - // Subscribe to Nav Survey-In - // Save off the gps pointer so we can use it in the callback later. - gps_ = gps; - gps->subscribe(std::bind( - &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); -} - -void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { - if (getRosBoolean(node_, "publish/nav/svin")) { - navsvin_pub_.publish(m); - } - - last_nav_svin_ = m; - - if (!m.active && m.valid && mode_ == SURVEY_IN) { - setTimeMode(gps_); - } - - updater_->update(); -} - -bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { - ROS_INFO("Setting mode (internal state) to Time Mode"); - mode_ = TIME; - - // Set the Measurement & nav rate to user config - // (survey-in sets nav_rate to 1 Hz regardless of user setting) - if (!gps->configRate(meas_rate_, nav_rate_)) { - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate_, - "navigation rate to ", nav_rate_); - } - // Enable the RTCM out messages - if (!gps->configRtcm(rtcms_)) { - ROS_ERROR("Failed to configure RTCM IDs"); - return false; - } - return true; -} - -void HpgRefProduct::initializeRosDiagnostics() { - updater_->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics); - updater_->force_update(); -} - -void HpgRefProduct::tmode3Diagnostics( - diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mode_ == INIT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Not configured"; - } else if (mode_ == DISABLED){ - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Disabled"; - } else if (mode_ == SURVEY_IN) { - if (!last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; - stat.message = "Survey-In inactive and invalid"; - } else if (last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; - stat.message = "Survey-In active but invalid"; - } else if (!last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Survey-In complete"; - } else if (last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Survey-In active and valid"; - } - - stat.add("iTOW [ms]", last_nav_svin_.i_tow); - stat.add("Duration [s]", last_nav_svin_.dur); - stat.add("# observations", last_nav_svin_.obs); - stat.add("Mean X [m]", last_nav_svin_.mean_x * 1e-2); - stat.add("Mean Y [m]", last_nav_svin_.mean_y * 1e-2); - stat.add("Mean Z [m]", last_nav_svin_.mean_z * 1e-2); - stat.add("Mean X HP [m]", last_nav_svin_.mean_xhp * 1e-4); - stat.add("Mean Y HP [m]", last_nav_svin_.mean_yhp * 1e-4); - stat.add("Mean Z HP [m]", last_nav_svin_.mean_zhp * 1e-4); - stat.add("Mean Accuracy [m]", last_nav_svin_.mean_acc * 1e-4); - } else if (mode_ == FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Fixed Position"; - } else if (mode_ == TIME) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; - stat.message = "Time"; - } -} - // // U-Blox High Precision Positioning Receiver // From 3cc1671224e07dcc85ec6c9617992dca8f783e02 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:42:12 +0000 Subject: [PATCH 077/152] Move HpPosRecProduct class to its own files. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- .../include/ublox_gps/hp_pos_rec_product.hpp | 51 ++++++++++++++ ublox_gps/include/ublox_gps/node.hpp | 29 -------- ublox_gps/src/hp_pos_rec_product.cpp | 70 +++++++++++++++++++ ublox_gps/src/node.cpp | 52 +------------- 5 files changed, 123 insertions(+), 81 deletions(-) create mode 100644 ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp create mode 100644 ublox_gps/src/hp_pos_rec_product.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 1f96956a..a1fc8d6d 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp src/hp_pos_rec_product.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp new file mode 100644 index 00000000..89a40943 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp @@ -0,0 +1,51 @@ +#ifndef UBLOX_GPS_HP_POS_REC_PRODUCT_HPP +#define UBLOX_GPS_HP_POS_REC_PRODUCT_HPP + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +class HpPosRecProduct final : public virtual HpgRefProduct { + public: + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); + + /** + * @brief Subscribe to Rover messages, such as NavRELPOSNED. + */ + void subscribe(std::shared_ptr gps) override; + + private: + + /** + * @brief Set the last received message and call rover diagnostic updater + * + * @details Publish received NavRELPOSNED messages if enabled + */ + void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); + + sensor_msgs::Imu imu_; + + //! Last relative position (used for diagnostic updater) + ublox_msgs::NavRELPOSNED9 last_rel_pos_; + + ros::Publisher nav_relposned_pub_; + ros::Publisher imu_pub_; + + std::string frame_id_; +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HP_POS_REC_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index d6fbe501..dd32eb2d 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -296,35 +296,6 @@ class UbloxNode final { }; -class HpPosRecProduct final : public virtual HpgRefProduct { - public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); - - /** - * @brief Subscribe to Rover messages, such as NavRELPOSNED. - */ - void subscribe(std::shared_ptr gps) override; - - private: - - /** - * @brief Set the last received message and call rover diagnostic updater - * - * @details Publish received NavRELPOSNED messages if enabled - */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); - - sensor_msgs::Imu imu_; - - //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED9 last_rel_pos_; - - ros::Publisher nav_relposned_pub_; - ros::Publisher imu_pub_; - - std::string frame_id_; -}; - } #endif // UBLOX_GPS_NODE_HPP diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp new file mode 100644 index 00000000..40f956a5 --- /dev/null +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -0,0 +1,70 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace ublox_node { + +// +// U-Blox High Precision Positioning Receiver +// +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) + : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) +{ + nav_relposned_pub_ = + node_->advertise("navrelposned", 1); + + imu_pub_ = + node_->advertise("navheading", 1); +} + +void HpPosRecProduct::subscribe(std::shared_ptr gps) { + // Whether to publish Nav Relative Position NED + // Subscribe to Nav Relative Position NED messages (also updates diagnostics) + gps->subscribe(std::bind( + &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); +} + +void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { + if (getRosBoolean(node_, "publish/nav/relposned")) { + nav_relposned_pub_.publish(m); + } + + if (getRosBoolean(node_, "publish/nav/heading")) { + imu_.header.stamp = ros::Time::now(); + imu_.header.frame_id = frame_id_; + + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; + + double heading = static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI; + tf2::Quaternion orientation; + orientation.setRPY(0, 0, heading); + imu_.orientation.x = orientation[0]; + imu_.orientation.y = orientation[1]; + imu_.orientation.z = orientation[2]; + imu_.orientation.w = orientation[3]; + // Only heading is reported with an accuracy in 0.1mm units + imu_.orientation_covariance[0] = 1000.0; + imu_.orientation_covariance[4] = 1000.0; + imu_.orientation_covariance[8] = pow(m.acc_heading / 10000.0, 2); + + imu_pub_.publish(imu_); + } + + last_rel_pos_ = m; + updater_->update(); +} + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index c2ca52dd..547f4fd3 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -785,57 +786,6 @@ void UbloxNode::shutdown() { } } -// -// U-Blox High Precision Positioning Receiver -// -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) - : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) -{ - nav_relposned_pub_ = - node_->advertise("navrelposned", 1); - - imu_pub_ = - node_->advertise("navheading", 1); -} - -void HpPosRecProduct::subscribe(std::shared_ptr gps) { - // Whether to publish Nav Relative Position NED - // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps->subscribe(std::bind( - &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); -} - -void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (getRosBoolean(node_, "publish/nav/relposned")) { - nav_relposned_pub_.publish(m); - } - - if (getRosBoolean(node_, "publish/nav/heading")) { - imu_.header.stamp = ros::Time::now(); - imu_.header.frame_id = frame_id_; - - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - double heading = static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI; - tf2::Quaternion orientation; - orientation.setRPY(0, 0, heading); - imu_.orientation.x = orientation[0]; - imu_.orientation.y = orientation[1]; - imu_.orientation.z = orientation[2]; - imu_.orientation.w = orientation[3]; - // Only heading is reported with an accuracy in 0.1mm units - imu_.orientation_covariance[0] = 1000.0; - imu_.orientation_covariance[4] = 1000.0; - imu_.orientation_covariance[8] = pow(m.acc_heading / 10000.0, 2); - - imu_pub_.publish(imu_); - } - - last_rel_pos_ = m; - updater_->update(); -} - } // namespace ublox_node int main(int argc, char** argv) { From 58b94ba39a4420365ff127f88a90b1a08ce7a6b4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 15:55:07 +0000 Subject: [PATCH 078/152] Cleanup includes in node.{hpp,cpp}. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 48 +++++-------------- .../include/ublox_gps/ublox_firmware.hpp | 4 ++ ublox_gps/src/node.cpp | 20 ++++++-- 3 files changed, 30 insertions(+), 42 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index dd32eb2d..bb304496 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -31,53 +31,28 @@ #define UBLOX_GPS_NODE_HPP // STL -#include #include -#include -#include #include #include // ROS includes #include -#include #include -#include -// ROS messages -#include -#include -#include -#include -#include -// Other U-Blox package includes -#include +// U-Blox msgs nicludes +#include +#include +#include // Ublox GPS includes -#include #include #include -#include #include -#include #include -#include -#include -#include -#include - -// This file also declares UbloxNode which implements ComponentInterface and is -// the main class and ros node. it implements functionality which applies to -// any u-blox device, regardless of the firmware version or product type. -// The class is designed in compositional style; it contains ComponentInterfaces -// which implement features specific to the device based on its firmware version -// and product category. UbloxNode calls the public methods of each component. -// -// This file declares UbloxFirmware is an abstract class which implements -// ComponentInterface and functions generic to all firmware (such as the -// initializing the fix diagnostics). Subclasses of UbloxFirmware for firmware -// versions 6-8 are also declared in this file. -// -// Lastly, this file declares classes for each product category which also -// implement u-blox interface, currently only the class for High Precision -// GNSS devices has been fully implemented and tested. + +// This file also declares UbloxNode which is the main class and ros node. It +// implements functionality which applies to any u-blox device, regardless of +// the firmware version or product type. The class is designed in compositional +// style; it contains ComponentInterfaces which implement features specific to +// the device based on its firmware version and product category. UbloxNode +// calls the public methods of each component. /** * @namespace ublox_node @@ -293,7 +268,6 @@ class UbloxNode final { //! Node Handle for GPS node std::shared_ptr nh_; - }; } diff --git a/ublox_gps/include/ublox_gps/ublox_firmware.hpp b/ublox_gps/include/ublox_gps/ublox_firmware.hpp index f8a6725e..0afea566 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware.hpp @@ -9,6 +9,10 @@ #include #include +// This file declares UbloxFirmware is an abstract class which implements +// ComponentInterface and functions generic to all firmware (such as the +// initializing the fix diagnostics). + namespace ublox_node { /** diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 547f4fd3..5dcaa94e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -27,16 +27,29 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== +#include +#include #include #include #include #include +#include #include #include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -48,13 +61,10 @@ #include #include #include -#include #include #include -#include #include #include -#include namespace ublox_node { From 8ba1df32cdec760099c7fc2aeb09a94eaf6297ee Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 16:13:42 +0000 Subject: [PATCH 079/152] Move spinning out of the constructor. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 4 ++++ ublox_gps/src/node.cpp | 17 ++++++++++------- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index bb304496..cff5ae62 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -97,6 +97,8 @@ class UbloxNode final { */ UbloxNode(); + ~UbloxNode(); + /** * @brief Get the node parameters from the ROS Parameter Server. */ @@ -268,6 +270,8 @@ class UbloxNode final { //! Node Handle for GPS node std::shared_ptr nh_; + + ros::Timer poller_; }; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 5dcaa94e..a73d5cf3 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -779,14 +779,10 @@ void UbloxNode::initialize() { // Configure INF messages (needs INF params, call after subscribing) configureInf(); - ros::Timer poller; - poller = nh_->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); - poller.start(); - ros::spin(); + poller_ = nh_->createTimer(ros::Duration(kPollDuration), + &UbloxNode::pollMessages, + this); } - shutdown(); } void UbloxNode::shutdown() { @@ -796,10 +792,17 @@ void UbloxNode::shutdown() { } } +UbloxNode::~UbloxNode() { + shutdown(); +} + } // namespace ublox_node int main(int argc, char** argv) { ros::init(argc, argv, "ublox_gps"); ublox_node::UbloxNode node; + + ros::spin(); + return 0; } From 787219bead36491ea155fa4cd013d6ffeb73fdf9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 16:15:54 +0000 Subject: [PATCH 080/152] Split the main out into its own file. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/node.hpp | 2 ++ ublox_gps/src/node.cpp | 9 --------- ublox_gps/src/node_main.cpp | 12 ++++++++++++ 4 files changed, 15 insertions(+), 10 deletions(-) create mode 100644 ublox_gps/src/node_main.cpp diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index a1fc8d6d..82747ec5 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -35,7 +35,7 @@ target_link_libraries(ublox_gps ) # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp src/hp_pos_rec_product.cpp) +add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp src/hp_pos_rec_product.cpp src/node_main.cpp) set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index cff5ae62..8d9802d9 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -42,7 +42,9 @@ #include #include // Ublox GPS includes +#include #include +#include #include #include #include diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index a73d5cf3..f49e4eea 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -797,12 +797,3 @@ UbloxNode::~UbloxNode() { } } // namespace ublox_node - -int main(int argc, char** argv) { - ros::init(argc, argv, "ublox_gps"); - ublox_node::UbloxNode node; - - ros::spin(); - - return 0; -} diff --git a/ublox_gps/src/node_main.cpp b/ublox_gps/src/node_main.cpp new file mode 100644 index 00000000..dac05883 --- /dev/null +++ b/ublox_gps/src/node_main.cpp @@ -0,0 +1,12 @@ +#include + +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "ublox_gps"); + ublox_node::UbloxNode node; + + ros::spin(); + + return 0; +} From 2bcb8413fbc0508910c9f75d91e143b9c57ccefb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 18:17:11 +0000 Subject: [PATCH 081/152] Start ROS 2 port by COLCON_IGNORE everything. Signed-off-by: Chris Lalancette --- ublox/COLCON_IGNORE | 0 ublox_gps/COLCON_IGNORE | 0 ublox_msgs/COLCON_IGNORE | 0 ublox_serialization/COLCON_IGNORE | 0 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ublox/COLCON_IGNORE create mode 100644 ublox_gps/COLCON_IGNORE create mode 100644 ublox_msgs/COLCON_IGNORE create mode 100644 ublox_serialization/COLCON_IGNORE diff --git a/ublox/COLCON_IGNORE b/ublox/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/ublox_gps/COLCON_IGNORE b/ublox_gps/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/ublox_msgs/COLCON_IGNORE b/ublox_msgs/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/ublox_serialization/COLCON_IGNORE b/ublox_serialization/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b From faff3904662db086acea2f89902fe383515cde2f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 18:21:54 +0000 Subject: [PATCH 082/152] Port of ublox_serialization to ROS 2. Signed-off-by: Chris Lalancette --- ublox_serialization/CMakeLists.txt | 15 ++- ublox_serialization/COLCON_IGNORE | 0 .../ublox_serialization/serialization.hpp | 97 +++---------------- ublox_serialization/package.xml | 12 +-- 4 files changed, 23 insertions(+), 101 deletions(-) delete mode 100644 ublox_serialization/COLCON_IGNORE diff --git a/ublox_serialization/CMakeLists.txt b/ublox_serialization/CMakeLists.txt index e8543e75..ca3bf732 100644 --- a/ublox_serialization/CMakeLists.txt +++ b/ublox_serialization/CMakeLists.txt @@ -1,14 +1,13 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_serialization) -find_package(catkin REQUIRED COMPONENTS roscpp roscpp_serialization) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp roscpp_serialization) +find_package(ament_cmake REQUIRED) install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE + DESTINATION include ) + +ament_export_include_directories(include) + +ament_package() diff --git a/ublox_serialization/COLCON_IGNORE b/ublox_serialization/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 813eab7e..7dcfc666 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -30,14 +30,12 @@ #define UBLOX_SERIALIZATION_SERIALIZATION_HPP #include +#include #include +#include #include #include -#include - -#include - #include "checksum.hpp" /// @@ -213,75 +211,6 @@ inline uint32_t serializationLength(const std::array& t) return StdArrayUbloxSerializer::serializedLength(t); } -/** - * \brief Array serializer, default implementation does nothing - */ -template -struct BoostArraySerializer -{}; - -/** - * \brief Array serializer, specialized for fixed-size, simple types - */ -template -struct BoostArraySerializer::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value || - std::is_same::value>::type> -{ - typedef boost::array ArrayType; - typedef typename ArrayType::iterator IteratorType; - typedef typename ArrayType::const_iterator ConstIteratorType; - - template - inline static void write(Stream& stream, const ArrayType& v) { - const uint32_t data_len = N * sizeof(T); - std::memcpy(stream.advance(data_len), &v.front(), data_len); - } - - template - inline static void read(Stream& stream, ArrayType& v) { - const uint32_t data_len = N * sizeof(T); - std::memcpy(&v.front(), stream.advance(data_len), data_len); - } - - inline static uint32_t serializedLength(const ArrayType& v) { - return N * sizeof(T); - } -}; - -/** - * \brief serialize version for std::array - */ -template -inline void serialize(Stream& stream, const boost::array& t) -{ - BoostArraySerializer::write(stream, t); -} - -/** - * \brief deserialize version for std::array - */ -template -inline void deserialize(Stream& stream, boost::array& t) { - BoostArraySerializer::read(stream, t); -} - -/** - * \brief serializationLength version for std::array - */ -template -inline uint32_t serializationLength(const boost::array& t) -{ - return BoostArraySerializer::serializedLength(t); -} - /** * \brief UbloxStream base-class, provides common functionality for UbloxIStream and UbloxOStream */ @@ -422,7 +351,7 @@ struct Options { * @brief Get the number of bytes in the header and footer. * @return the number of bytes in the header and footer */ - int wrapper_length() { + uint32_t wrapper_length() { return header_length + checksum_length; } }; @@ -564,8 +493,8 @@ class Reader { uint16_t chk; if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { // checksum error - ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), - messageId()); + // ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), + // messageId()); return false; } @@ -638,8 +567,8 @@ class Writer { // Check for buffer overflow uint32_t length = UbloxSerializer::serializedLength(message); if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + // class_id, message_id); return false; } // Encode the message and add it to the buffer @@ -660,8 +589,8 @@ class Writer { bool write(const uint8_t* message, uint32_t length, uint8_t class_id, uint8_t message_id) { if (size_ < length + options_.wrapper_length()) { - ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", - class_id, message_id); + // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", + // class_id, message_id); return false; } iterator start = data_; @@ -710,10 +639,10 @@ class Writer { // Use to declare u-blox messages and message serializers #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \ - template class ublox::UbloxSerializer; \ - template class ublox::Message; \ + template class ublox::UbloxSerializer; \ + template class ublox::Message; \ namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \ } } \ // Use for messages which have the same structure but different IDs, e.g. INF @@ -721,7 +650,7 @@ class Writer { // for following declarations #define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \ namespace package { namespace { \ - static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ + static const ublox::Message::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \ } } \ #endif // UBLOX_SERIALIZATION_SERIALIZATION_HPP diff --git a/ublox_serialization/package.xml b/ublox_serialization/package.xml index 867ccd14..f5af37b0 100644 --- a/ublox_serialization/package.xml +++ b/ublox_serialization/package.xml @@ -1,5 +1,5 @@ - + ublox_serialization 1.2.0 @@ -12,16 +12,10 @@ BSD http://ros.org/wiki/ublox - catkin - - roscpp - roscpp - - roscpp_serialization - roscpp_serialization + ament_cmake - + ament_cmake From be59d849774c42fac823ccad75e148ba55e9f2fc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 18:23:29 +0000 Subject: [PATCH 083/152] Port ublox_msgs to ROS 2. Signed-off-by: Chris Lalancette --- ublox_msgs/CMakeLists.txt | 135 +++- ublox_msgs/COLCON_IGNORE | 0 .../include/ublox_msgs/serialization.hpp | 619 +++++++++--------- ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 260 ++++---- ublox_msgs/package.xml | 22 +- ublox_msgs/src/ublox_msgs.cpp | 138 ++-- 6 files changed, 630 insertions(+), 544 deletions(-) delete mode 100644 ublox_msgs/COLCON_IGNORE diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 3dbf3dab..46a08336 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -1,37 +1,126 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation ublox_serialization std_msgs sensor_msgs) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_message_files(DIRECTORY msg) -generate_messages(DEPENDENCIES std_msgs sensor_msgs) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS message_runtime ublox_serialization std_msgs sensor_msgs) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ublox_serialization REQUIRED) -include_directories(${PROJECT_SOURCE_DIR}/include) -include_directories(${ublox_serialization_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} src/ublox_msgs.cpp) +set(msg_files + "msg/Ack.msg" + "msg/AidALM.msg" + "msg/AidEPH.msg" + "msg/AidHUI.msg" + "msg/CfgANT.msg" + "msg/CfgCFG.msg" + "msg/CfgDAT.msg" + "msg/CfgDGNSS.msg" + "msg/CfgGNSSBlock.msg" + "msg/CfgGNSS.msg" + "msg/CfgHNR.msg" + "msg/CfgINFBlock.msg" + "msg/CfgINF.msg" + "msg/CfgMSG.msg" + "msg/CfgNAV5.msg" + "msg/CfgNAVX5.msg" + "msg/CfgNMEA6.msg" + "msg/CfgNMEA7.msg" + "msg/CfgNMEA.msg" + "msg/CfgPRT.msg" + "msg/CfgRATE.msg" + "msg/CfgRST.msg" + "msg/CfgSBAS.msg" + "msg/CfgTMODE3.msg" + "msg/CfgUSB.msg" + "msg/EsfINS.msg" + "msg/EsfMEAS.msg" + "msg/EsfRAWBlock.msg" + "msg/EsfRAW.msg" + "msg/EsfSTATUS.msg" + "msg/EsfSTATUSSens.msg" + "msg/HnrPVT.msg" + "msg/Inf.msg" + "msg/MgaGAL.msg" + "msg/MonGNSS.msg" + "msg/MonHW6.msg" + "msg/MonHW.msg" + "msg/MonVERExtension.msg" + "msg/MonVER.msg" + "msg/NavATT.msg" + "msg/NavCLOCK.msg" + "msg/NavDGPS.msg" + "msg/NavDGPSSV.msg" + "msg/NavDOP.msg" + "msg/NavPOSECEF.msg" + "msg/NavPOSLLH.msg" + "msg/NavPVT7.msg" + "msg/NavPVT.msg" + "msg/NavRELPOSNED9.msg" + "msg/NavRELPOSNED.msg" + "msg/NavSAT.msg" + "msg/NavSATSV.msg" + "msg/NavSBAS.msg" + "msg/NavSBASSV.msg" + "msg/NavSOL.msg" + "msg/NavSTATUS.msg" + "msg/NavSVINFO.msg" + "msg/NavSVINFOSV.msg" + "msg/NavSVIN.msg" + "msg/NavTIMEGPS.msg" + "msg/NavTIMEUTC.msg" + "msg/NavVELECEF.msg" + "msg/NavVELNED.msg" + "msg/RxmALM.msg" + "msg/RxmEPH.msg" + "msg/RxmRAW.msg" + "msg/RxmRAWSV.msg" + "msg/RxmRAWXMeas.msg" + "msg/RxmRAWX.msg" + "msg/RxmRTCM.msg" + "msg/RxmSFRB.msg" + "msg/RxmSFRBX.msg" + "msg/RxmSVSI.msg" + "msg/RxmSVSISV.msg" + "msg/TimTM2.msg" + "msg/UpdSOSAck.msg" + "msg/UpdSOS.msg" +) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES + sensor_msgs + std_msgs ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +include_directories(include) +add_library(${PROJECT_NAME}_lib SHARED src/ublox_msgs.cpp) +ament_target_dependencies(${PROJECT_NAME}_lib + "ublox_serialization" ) +rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_lib) +ament_export_dependencies(rosidl_default_runtime std_msgs sensor_msgs ublox_serialization) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME}_lib + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE + DESTINATION include ) + +ament_package() diff --git a/ublox_msgs/COLCON_IGNORE b/ublox_msgs/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 3dc0bdd0..9ef607fc 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -32,7 +32,6 @@ #include -#include #include #include @@ -45,21 +44,21 @@ namespace ublox { template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::Ack_ &m) { + ublox_msgs::msg::Ack_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.cls_id); stream.next(m.msg_id); } - inline static uint32_t serializedLength(const ublox_msgs::Ack_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::Ack_ &m) { (void)m; return 2; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::Ack_ &m) { + const ublox_msgs::msg::Ack_ &m) { UbloxOStream stream(data, size); stream.next(m.cls_id); stream.next(m.msg_id); @@ -70,16 +69,16 @@ struct UbloxSerializer > { /// @brief Serializes the AidALM message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::AidALM_ &m) { + ublox_msgs::msg::AidALM_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); if (count == 40) { - typename ublox_msgs::AidALM_::_dwrd_type::value_type temp; + typename ublox_msgs::msg::AidALM_::_dwrd_type::value_type temp; m.dwrd.resize(8); for (std::size_t i = 0; i < 8; ++i) { stream.next(temp); @@ -88,12 +87,12 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::AidALM_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::AidALM_ &m) { return 8 + (4 * m.dwrd.size()); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::AidALM_ &m) { + const ublox_msgs::msg::AidALM_ &m) { UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.week); @@ -107,10 +106,10 @@ struct UbloxSerializer > { /// @brief Serializes the AidEPH message which has a repeated block. /// template -struct UbloxSerializer > +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::AidEPH_ &m) { + ublox_msgs::msg::AidEPH_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); @@ -119,9 +118,9 @@ struct UbloxSerializer > m.sf3d.clear(); if (count == 104) { - typename ublox_msgs::AidEPH_::_sf1d_type::value_type temp1; - typename ublox_msgs::AidEPH_::_sf2d_type::value_type temp2; - typename ublox_msgs::AidEPH_::_sf3d_type::value_type temp3; + typename ublox_msgs::msg::AidEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::msg::AidEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::msg::AidEPH_::_sf3d_type::value_type temp3; m.sf1d.resize(8); for (std::size_t i = 0; i < 8; ++i) { stream.next(temp1); @@ -140,12 +139,12 @@ struct UbloxSerializer > } } - inline static uint32_t serializedLength(const ublox_msgs::AidEPH_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::AidEPH_ &m) { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::AidEPH_ &m) { + const ublox_msgs::msg::AidEPH_ &m) { UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.how); @@ -162,9 +161,9 @@ struct UbloxSerializer > }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::AidHUI_ &m) { + ublox_msgs::msg::AidHUI_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.health); stream.next(m.utc_a0); @@ -185,13 +184,13 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::AidHUI_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::AidHUI_ &m) { (void)m; return 72; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::AidHUI_ &m) { + const ublox_msgs::msg::AidHUI_ &m) { UbloxOStream stream(data, size); stream.next(m.health); stream.next(m.utc_a0); @@ -214,9 +213,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgCFG_ &m) { + ublox_msgs::msg::CfgCFG_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.clear_mask); stream.next(m.save_mask); @@ -224,13 +223,13 @@ struct UbloxSerializer > { stream.next(m.device_mask); } - inline static uint32_t serializedLength(const ublox_msgs::CfgCFG_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgCFG_ &m) { (void)m; return 13; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgCFG_ &m) { + const ublox_msgs::msg::CfgCFG_ &m) { UbloxOStream stream(data, size); stream.next(m.clear_mask); stream.next(m.save_mask); @@ -244,9 +243,9 @@ struct UbloxSerializer > { /// get/set. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgDAT_ &m) { + ublox_msgs::msg::CfgDAT_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.datum_num); stream.next(m.datum_name); @@ -261,7 +260,7 @@ struct UbloxSerializer > { stream.next(m.scale); } - inline static uint32_t serializedLength(const ublox_msgs::CfgDAT_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgDAT_ &m) { // this is the size of CfgDAT set messages // serializedLength is only used for writes so this is ok (void)m; @@ -269,7 +268,7 @@ struct UbloxSerializer > { } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgDAT_ &m) { + const ublox_msgs::msg::CfgDAT_ &m) { UbloxOStream stream(data, size); // ignores datumNum & datumName stream.next(m.maj_a); @@ -285,9 +284,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgDGNSS_ &m) { + ublox_msgs::msg::CfgDGNSS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.dgnss_mode); stream.next(m.reserved0[0]); @@ -295,13 +294,13 @@ struct UbloxSerializer > { stream.next(m.reserved0[2]); } - inline static uint32_t serializedLength(const ublox_msgs::CfgDGNSS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgDGNSS_ &m) { (void)m; return 4; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgDGNSS_ &m) { + const ublox_msgs::msg::CfgDGNSS_ &m) { UbloxOStream stream(data, size); stream.next(m.dgnss_mode); stream.next(m.reserved0[0]); @@ -311,8 +310,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::CfgGNSSBlock_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::CfgGNSSBlock_ & m) { stream.next(m.gnss_id); stream.next(m.res_trk_ch); stream.next(m.max_trk_ch); @@ -320,12 +319,12 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::CfgGNSSBlock_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgGNSSBlock_ & m) { (void)m; return 8; } - inline static void write(UbloxOStream& stream, const ublox_msgs::CfgGNSSBlock_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::CfgGNSSBlock_ & m) { stream.next(m.gnss_id); stream.next(m.res_trk_ch); stream.next(m.max_trk_ch); @@ -338,9 +337,9 @@ struct UbloxSerializer > { /// @brief Serializes the CfgGNSS message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgGNSS_ &m) { + ublox_msgs::msg::CfgGNSS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.msg_ver); stream.next(m.num_trk_ch_hw); @@ -352,21 +351,21 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::CfgGNSS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgGNSS_ &m) { return 4 + 8 * m.num_config_blocks; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgGNSS_ &m) { + const ublox_msgs::msg::CfgGNSS_ &m) { if (m.blocks.size() != m.num_config_blocks) { - ROS_ERROR("CfgGNSS num_config_blocks must equal blocks size"); + // ROS_ERROR("CfgGNSS num_config_blocks must equal blocks size"); } UbloxOStream stream(data, size); stream.next(m.msg_ver); stream.next(m.num_trk_ch_hw); stream.next(m.num_trk_ch_use); stream.next( - static_cast::_num_config_blocks_type>(m.blocks.size())); + static_cast::_num_config_blocks_type>(m.blocks.size())); for (std::size_t i = 0; i < m.blocks.size(); ++i) { serialize(stream, m.blocks[i]); } @@ -374,8 +373,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::CfgINFBlock_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::CfgINFBlock_ & m) { stream.next(m.protocol_id); stream.next(m.reserved1[0]); stream.next(m.reserved1[1]); @@ -388,12 +387,12 @@ struct UbloxSerializer > { stream.next(m.inf_msg_mask[5]); } - inline static uint32_t serializedLength(const ublox_msgs::CfgINFBlock_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgINFBlock_ & m) { (void)m; return 10; } - inline static void write(UbloxOStream& stream, const ublox_msgs::CfgINFBlock_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::CfgINFBlock_ & m) { stream.next(m.protocol_id); stream.next(m.reserved1[0]); stream.next(m.reserved1[1]); @@ -411,23 +410,23 @@ struct UbloxSerializer > { /// @brief Serializes the CfgInf message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgINF_ &m) { + ublox_msgs::msg::CfgINF_ &m) { UbloxIStream stream(const_cast(data), count); - int num_blocks = count / 10; + uint32_t num_blocks = count / 10; m.blocks.resize(num_blocks); for (std::size_t i = 0; i < num_blocks; ++i) { deserialize(stream, m.blocks[i]); } } - inline static uint32_t serializedLength(const ublox_msgs::CfgINF_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgINF_ &m) { return 10 * m.blocks.size(); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgINF_ &m) { + const ublox_msgs::msg::CfgINF_ &m) { UbloxOStream stream(data, size); for (std::size_t i = 0; i < m.blocks.size(); ++i) { serialize(stream, m.blocks[i]); @@ -436,22 +435,22 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgMSG_ &m) { + ublox_msgs::msg::CfgMSG_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.msg_class); stream.next(m.msg_id); stream.next(m.rate); } - inline static uint32_t serializedLength(const ublox_msgs::CfgMSG_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgMSG_ &m) { (void)m; return 3; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgMSG_ &m) { + const ublox_msgs::msg::CfgMSG_ &m) { UbloxOStream stream(data, size); stream.next(m.msg_class); stream.next(m.msg_id); @@ -460,9 +459,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgNAV5_ &m) { + ublox_msgs::msg::CfgNAV5_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.mask); stream.next(m.dyn_model); @@ -490,13 +489,13 @@ struct UbloxSerializer > { stream.next(m.reserved2[4]); } - inline static uint32_t serializedLength(const ublox_msgs::CfgNAV5_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNAV5_ &m) { (void)m; return 36; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgNAV5_ &m) { + const ublox_msgs::msg::CfgNAV5_ &m) { UbloxOStream stream(data, size); stream.next(m.mask); stream.next(m.dyn_model); @@ -526,9 +525,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgNAVX5_ &m) { + ublox_msgs::msg::CfgNAVX5_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.mask1); @@ -565,13 +564,13 @@ struct UbloxSerializer > { stream.next(m.use_adr); } - inline static uint32_t serializedLength(const ublox_msgs::CfgNAVX5_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNAVX5_ &m) { (void)m; return 40; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgNAVX5_ &m) { + const ublox_msgs::msg::CfgNAVX5_ &m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.mask1); @@ -610,9 +609,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgNMEA_ &m) { + ublox_msgs::msg::CfgNMEA_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.filter); stream.next(m.nmea_version); @@ -633,13 +632,13 @@ struct UbloxSerializer > { stream.next(m.reserved1[5]); } - inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA_ &m) { (void)m; return 20; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgNMEA_ &m) { + const ublox_msgs::msg::CfgNMEA_ &m) { UbloxOStream stream(data, size); stream.next(m.filter); stream.next(m.nmea_version); @@ -662,9 +661,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgNMEA6_ &m) { + ublox_msgs::msg::CfgNMEA6_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.filter); stream.next(m.version); @@ -672,13 +671,13 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA6_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA6_ &m) { (void)m; return 4; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgNMEA6_ &m) { + const ublox_msgs::msg::CfgNMEA6_ &m) { UbloxOStream stream(data, size); stream.next(m.filter); stream.next(m.version); @@ -688,9 +687,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgNMEA7_ &m) { + ublox_msgs::msg::CfgNMEA7_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.filter); stream.next(m.nmea_version); @@ -703,13 +702,13 @@ struct UbloxSerializer > { stream.next(m.reserved); } - inline static uint32_t serializedLength(const ublox_msgs::CfgNMEA7_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgNMEA7_ &m) { (void)m; return 12; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgNMEA7_ &m) { + const ublox_msgs::msg::CfgNMEA7_ &m) { UbloxOStream stream(data, size); stream.next(m.filter); stream.next(m.nmea_version); @@ -724,9 +723,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgPRT_ &m) { + ublox_msgs::msg::CfgPRT_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.port_id); stream.next(m.reserved0); @@ -739,13 +738,13 @@ struct UbloxSerializer > { stream.next(m.reserved1); } - inline static uint32_t serializedLength(const ublox_msgs::CfgPRT_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgPRT_ &m) { (void)m; return 20; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgPRT_ &m) { + const ublox_msgs::msg::CfgPRT_ &m) { UbloxOStream stream(data, size); stream.next(m.port_id); stream.next(m.reserved0); @@ -760,22 +759,22 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgRATE_ &m) { + ublox_msgs::msg::CfgRATE_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.meas_rate); stream.next(m.nav_rate); stream.next(m.time_ref); } - inline static uint32_t serializedLength(const ublox_msgs::CfgRATE_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgRATE_ &m) { (void)m; return 6; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgRATE_ &m) { + const ublox_msgs::msg::CfgRATE_ &m) { UbloxOStream stream(data, size); stream.next(m.meas_rate); stream.next(m.nav_rate); @@ -784,22 +783,22 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgRST_ &m) { + ublox_msgs::msg::CfgRST_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.nav_bbr_mask); stream.next(m.reset_mode); stream.next(m.reserved1); } - inline static uint32_t serializedLength(const ublox_msgs::CfgRST_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgRST_ &m) { (void)m; return 4; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgRST_ &m) { + const ublox_msgs::msg::CfgRST_ &m) { UbloxOStream stream(data, size); stream.next(m.nav_bbr_mask); stream.next(m.reset_mode); @@ -808,9 +807,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgSBAS_ &m) { + ublox_msgs::msg::CfgSBAS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.mode); stream.next(m.usage); @@ -819,13 +818,13 @@ struct UbloxSerializer > { stream.next(m.scanmode1); } - inline static uint32_t serializedLength(const ublox_msgs::CfgSBAS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgSBAS_ &m) { (void)m; return 8; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgSBAS_ &m) { + const ublox_msgs::msg::CfgSBAS_ &m) { UbloxOStream stream(data, size); stream.next(m.mode); stream.next(m.usage); @@ -836,9 +835,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::CfgTMODE3_ &m) { + ublox_msgs::msg::CfgTMODE3_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.reserved1); @@ -863,13 +862,13 @@ struct UbloxSerializer > { stream.next(m.reserved3[7]); } - inline static uint32_t serializedLength(const ublox_msgs::CfgTMODE3_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgTMODE3_ &m) { (void)m; return 40; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::CfgTMODE3_ &m) { + const ublox_msgs::msg::CfgTMODE3_ &m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.reserved1); @@ -896,9 +895,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfINS_ & m) { + ublox_msgs::msg::EsfINS_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.bitfield0); stream.next(m.reserved1[0]); @@ -914,13 +913,13 @@ struct UbloxSerializer > { stream.next(m.z_accel); } - inline static uint32_t serializedLength(const ublox_msgs::EsfINS_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfINS_ & m) { (void)m; return 36; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfINS_ & m) { + const ublox_msgs::msg::EsfINS_ & m) { UbloxOStream stream(data, size); stream.next(m.bitfield0); stream.next(m.reserved1[0]); @@ -942,16 +941,16 @@ struct UbloxSerializer > { /// optional block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfMEAS_ &m) { + ublox_msgs::msg::EsfMEAS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.time_tag); stream.next(m.flags); stream.next(m.id); bool calib_valid = m.flags & m.FLAGS_CALIB_T_TAG_VALID; - int data_size = (count - (calib_valid ? 12 : 8)) / 4; + uint32_t data_size = (count - (calib_valid ? 12 : 8)) / 4; // Repeating block m.data.resize(data_size); for (std::size_t i = 0; i < data_size; ++i) { @@ -964,12 +963,12 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::EsfMEAS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfMEAS_ &m) { return 4 + 8 * m.data.size() + 4 * m.calib_t_tag.size(); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfMEAS_ &m) { + const ublox_msgs::msg::EsfMEAS_ &m) { UbloxOStream stream(data, size); stream.next(m.time_tag); stream.next(m.flags); @@ -984,18 +983,18 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::EsfRAWBlock_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::EsfRAWBlock_ & m) { stream.next(m.data); stream.next(m.s_t_tag); } - inline static uint32_t serializedLength(const ublox_msgs::EsfRAWBlock_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfRAWBlock_ & m) { (void)m; return 8; } - inline static void write(UbloxOStream& stream, const ublox_msgs::EsfRAWBlock_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::EsfRAWBlock_ & m) { stream.next(m.data); stream.next(m.s_t_tag); } @@ -1005,25 +1004,25 @@ struct UbloxSerializer > { /// @brief Serializes the EsfRAW message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfRAW_ &m) { + ublox_msgs::msg::EsfRAW_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.reserved0); m.blocks.clear(); - int num_blocks = (count - 4) / 8; + uint32_t num_blocks = (count - 4) / 8; m.blocks.resize(num_blocks); for (std::size_t i = 0; i < num_blocks; ++i) { deserialize(stream, m.blocks[i]); } } - inline static uint32_t serializedLength(const ublox_msgs::EsfRAW_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfRAW_ &m) { return 4 + 8 * m.blocks.size(); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfRAW_ &m) { + const ublox_msgs::msg::EsfRAW_ &m) { UbloxOStream stream(data, size); stream.next(m.reserved0); for (std::size_t i = 0; i < m.blocks.size(); ++i) { @@ -1033,20 +1032,20 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::EsfSTATUSSens_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::EsfSTATUSSens_ & m) { stream.next(m.sens_status1); stream.next(m.sens_status2); stream.next(m.freq); stream.next(m.faults); } - inline static uint32_t serializedLength(const ublox_msgs::EsfSTATUSSens_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfSTATUSSens_ & m) { (void)m; return 4; } - inline static void write(UbloxOStream& stream, const ublox_msgs::EsfSTATUSSens_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::EsfSTATUSSens_ & m) { stream.next(m.sens_status1); stream.next(m.sens_status2); stream.next(m.freq); @@ -1058,9 +1057,9 @@ struct UbloxSerializer > { /// @brief Serializes the EsfSTATUS message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::EsfSTATUS_ &m) { + ublox_msgs::msg::EsfSTATUS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.version); @@ -1073,21 +1072,21 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::EsfSTATUS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::EsfSTATUS_ &m) { return 16 + 4 * m.num_sens; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::EsfSTATUS_ &m) { + const ublox_msgs::msg::EsfSTATUS_ &m) { if (m.sens.size() != m.num_sens) { - ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); + // ROS_ERROR("Writing EsfSTATUS message: num_sens must equal size of sens"); } UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.version); stream.next(m.fusion_mode); stream.next(m.reserved2); - stream.next(static_cast::_num_sens_type>(m.sens.size())); + stream.next(static_cast::_num_sens_type>(m.sens.size())); for (std::size_t i = 0; i < m.sens.size(); ++i) { serialize(stream, m.sens[i]); } @@ -1095,9 +1094,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::HnrPVT_ & m) { + ublox_msgs::msg::HnrPVT_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.year); @@ -1130,13 +1129,13 @@ struct UbloxSerializer > { stream.next(m.reserved1[3]); } - inline static uint32_t serializedLength(const ublox_msgs::HnrPVT_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::HnrPVT_ & m) { (void)m; return 72; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::HnrPVT_ & m) { + const ublox_msgs::msg::HnrPVT_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.year); @@ -1174,22 +1173,22 @@ struct UbloxSerializer > { /// @brief Serializes the Inf message which has a dynamic length string. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::Inf_ &m) { + ublox_msgs::msg::Inf_ &m) { UbloxIStream stream(const_cast(data), count); m.str.resize(count); - for (int i = 0; i < count; ++i) { + for (uint32_t i = 0; i < count; ++i) { deserialize(stream, m.str[i]); } } - inline static uint32_t serializedLength(const ublox_msgs::Inf_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::Inf_ &m) { return m.str.size(); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::Inf_ &m) { + const ublox_msgs::msg::Inf_ &m) { UbloxOStream stream(data, size); for (std::size_t i = 0; i < m.str.size(); ++i) { serialize(stream, m.str[i]); @@ -1198,9 +1197,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::MonHW_ & m) { + ublox_msgs::msg::MonHW_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.pin_sel); stream.next(m.pin_bank); @@ -1238,13 +1237,13 @@ struct UbloxSerializer > { stream.next(m.pull_l); } - inline static uint32_t serializedLength(const ublox_msgs::MonHW_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::MonHW_ & m) { (void)m; return 60; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::MonHW_ & m) { + const ublox_msgs::msg::MonHW_ & m) { UbloxOStream stream(data, size); stream.next(m.pin_sel); stream.next(m.pin_bank); @@ -1284,9 +1283,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::MonHW6_ & m) { + ublox_msgs::msg::MonHW6_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.pin_sel); stream.next(m.pin_bank); @@ -1332,13 +1331,13 @@ struct UbloxSerializer > { stream.next(m.pull_l); } - inline static uint32_t serializedLength(const ublox_msgs::MonHW6_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::MonHW6_ & m) { (void)m; return 68; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::MonHW6_ & m) { + const ublox_msgs::msg::MonHW6_ & m) { UbloxOStream stream(data, size); stream.next(m.pin_sel); stream.next(m.pin_bank); @@ -1386,17 +1385,17 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::MonVERExtension_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::MonVERExtension_ & m) { stream.next(m.field); } - inline static uint32_t serializedLength(const ublox_msgs::MonVERExtension_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::MonVERExtension_ & m) { (void)m; return 30; } - inline static void write(UbloxOStream& stream, const ublox_msgs::MonVERExtension_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::MonVERExtension_ & m) { stream.next(m.field); } }; @@ -1405,9 +1404,9 @@ struct UbloxSerializer > { /// @brief Serializes the MonVER message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::MonVER_ &m) { + ublox_msgs::msg::MonVER_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.sw_version); stream.next(m.hw_version); @@ -1415,19 +1414,19 @@ struct UbloxSerializer > { m.extension.clear(); int N = (count - 40) / 30; m.extension.resize(N); - typename ublox_msgs::MonVER_::_extension_type::value_type ext; + typename ublox_msgs::msg::MonVER_::_extension_type::value_type ext; for (int i = 0; i < N; i++) { // Read each extension string deserialize(stream, m.extension[i]); } } - inline static uint32_t serializedLength(const ublox_msgs::MonVER_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::MonVER_ &m) { return 40 + (30 * m.extension.size()); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::MonVER_ &m) { + const ublox_msgs::msg::MonVER_ &m) { UbloxOStream stream(data, size); stream.next(m.sw_version); stream.next(m.hw_version); @@ -1438,9 +1437,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavATT_ & m) { + ublox_msgs::msg::NavATT_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.version); @@ -1455,13 +1454,13 @@ struct UbloxSerializer > { stream.next(m.acc_heading); } - inline static uint32_t serializedLength(const ublox_msgs::NavATT_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavATT_ & m) { (void)m; return 32; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavATT_ & m) { + const ublox_msgs::msg::NavATT_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.version); @@ -1478,9 +1477,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavCLOCK_ & m) { + ublox_msgs::msg::NavCLOCK_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.clk_b); @@ -1489,13 +1488,13 @@ struct UbloxSerializer > { stream.next(m.f_acc); } - inline static uint32_t serializedLength(const ublox_msgs::NavCLOCK_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavCLOCK_ & m) { (void)m; return 20; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavCLOCK_ & m) { + const ublox_msgs::msg::NavCLOCK_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.clk_b); @@ -1506,8 +1505,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::NavDGPSSV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavDGPSSV_ & m) { stream.next(m.svid); stream.next(m.flags); stream.next(m.age_c); @@ -1515,12 +1514,12 @@ struct UbloxSerializer > { stream.next(m.prrc); } - inline static uint32_t serializedLength(const ublox_msgs::NavDGPSSV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavDGPSSV_ & m) { (void)m; return 12; } - inline static void write(UbloxOStream& stream, const ublox_msgs::NavDGPSSV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavDGPSSV_ & m) { stream.next(m.svid); stream.next(m.flags); stream.next(m.age_c); @@ -1533,9 +1532,9 @@ struct UbloxSerializer > { /// @brief Serializes the NavDGPS message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavDGPS_ &m) { + ublox_msgs::msg::NavDGPS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.age); @@ -1550,21 +1549,21 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::NavDGPS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavDGPS_ &m) { return 16 + 12 * m.num_ch; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavDGPS_ &m) { - if (m.sv.size() != m.num_ch) { - ROS_ERROR("NavDGPS numCh must equal sv size"); + const ublox_msgs::msg::NavDGPS_ &m) { + if (m.sv.size() != static_cast(m.num_ch)) { + // ROS_ERROR("NavDGPS numCh must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.age); stream.next(m.base_id); stream.next(m.base_health); - stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(static_cast::_num_ch_type>(m.sv.size())); stream.next(m.status); stream.next(m.reserved1); for (std::size_t i = 0; i < m.sv.size(); ++i) { @@ -1574,9 +1573,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavPOSECEF_ & m) { + ublox_msgs::msg::NavPOSECEF_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.ecef_x); @@ -1585,13 +1584,13 @@ struct UbloxSerializer > { stream.next(m.p_acc); } - inline static uint32_t serializedLength(const ublox_msgs::NavPOSECEF_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPOSECEF_ & m) { (void)m; return 20; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavPOSECEF_ & m) { + const ublox_msgs::msg::NavPOSECEF_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.ecef_x); @@ -1602,9 +1601,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavPOSLLH_ & m) { + ublox_msgs::msg::NavPOSLLH_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.lon); @@ -1615,13 +1614,13 @@ struct UbloxSerializer > { stream.next(m.v_acc); } - inline static uint32_t serializedLength(const ublox_msgs::NavPOSLLH_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPOSLLH_ & m) { (void)m; return 28; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavPOSLLH_ & m) { + const ublox_msgs::msg::NavPOSLLH_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.lon); @@ -1634,9 +1633,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavPVT_ & m) { + ublox_msgs::msg::NavPVT_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.year); @@ -1677,13 +1676,13 @@ struct UbloxSerializer > { stream.next(m.mag_acc); } - inline static uint32_t serializedLength(const ublox_msgs::NavPVT_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPVT_ & m) { (void)m; return 92; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavPVT_ & m) { + const ublox_msgs::msg::NavPVT_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.year); @@ -1726,9 +1725,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavPVT7_ & m) { + ublox_msgs::msg::NavPVT7_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.year); @@ -1766,13 +1765,13 @@ struct UbloxSerializer > { stream.next(m.reserved1[5]); } - inline static uint32_t serializedLength(const ublox_msgs::NavPVT7_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavPVT7_ & m) { (void)m; return 84; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavPVT7_ & m) { + const ublox_msgs::msg::NavPVT7_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.year); @@ -1812,9 +1811,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavRELPOSNED_ & m) { + ublox_msgs::msg::NavRELPOSNED_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.reserved0); @@ -1833,13 +1832,13 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::NavRELPOSNED_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavRELPOSNED_ & m) { (void)m; return 40; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavRELPOSNED_ & m) { + const ublox_msgs::msg::NavRELPOSNED_ & m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.reserved0); @@ -1860,9 +1859,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavRELPOSNED9_ & m) { + ublox_msgs::msg::NavRELPOSNED9_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.reserved1); @@ -1893,13 +1892,13 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::NavRELPOSNED9_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavRELPOSNED9_ & m) { (void)m; return 64; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavRELPOSNED9_ & m) { + const ublox_msgs::msg::NavRELPOSNED9_ & m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.reserved1); @@ -1932,8 +1931,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::NavSATSV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSATSV_ & m) { stream.next(m.gnss_id); stream.next(m.sv_id); stream.next(m.cno); @@ -1943,12 +1942,12 @@ struct UbloxSerializer > { stream.next(m.flags); } - inline static uint32_t serializedLength(const ublox_msgs::NavSATSV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSATSV_ & m) { (void)m; return 12; } - inline static void write(UbloxOStream& stream, const ublox_msgs::NavSATSV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSATSV_ & m) { stream.next(m.gnss_id); stream.next(m.sv_id); stream.next(m.cno); @@ -1963,9 +1962,9 @@ struct UbloxSerializer > { /// @brief Serializes the NavSAT message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSAT_ &m) { + ublox_msgs::msg::NavSAT_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.version); @@ -1977,19 +1976,19 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::NavSAT_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSAT_ &m) { return 8 + 12 * m.num_svs; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSAT_ &m) { + const ublox_msgs::msg::NavSAT_ &m) { if (m.sv.size() != m.num_svs) { - ROS_ERROR("NavSAT num_svs must equal sv size"); + // ROS_ERROR("NavSAT num_svs must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.version); - stream.next(static_cast::_num_svs_type>(m.sv.size())); + stream.next(static_cast::_num_svs_type>(m.sv.size())); stream.next(m.reserved0); for (std::size_t i = 0; i < m.sv.size(); ++i) { serialize(stream, m.sv[i]); @@ -1998,8 +1997,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::NavSBASSV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSBASSV_ & m) { stream.next(m.svid); stream.next(m.flags); stream.next(m.udre); @@ -2011,12 +2010,12 @@ struct UbloxSerializer > { stream.next(m.ic); } - inline static uint32_t serializedLength(const ublox_msgs::NavSBASSV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSBASSV_ & m) { (void)m; return 12; } - inline static void write(UbloxOStream& stream, const ublox_msgs::NavSBASSV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSBASSV_ & m) { stream.next(m.svid); stream.next(m.flags); stream.next(m.udre); @@ -2033,9 +2032,9 @@ struct UbloxSerializer > { /// @brief Serializes the NavSBAS message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSBAS_ &m) { + ublox_msgs::msg::NavSBAS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.geo); @@ -2050,14 +2049,14 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::NavSBAS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSBAS_ &m) { return 12 + 12 * m.cnt; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSBAS_ &m) { + const ublox_msgs::msg::NavSBAS_ &m) { if (m.sv.size() != m.cnt) { - ROS_ERROR("NavSBAS cnt must equal sv size"); + // ROS_ERROR("NavSBAS cnt must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.i_tow); @@ -2065,7 +2064,7 @@ struct UbloxSerializer > { stream.next(m.mode); stream.next(m.sys); stream.next(m.service); - stream.next(static_cast::_cnt_type>(m.sv.size())); + stream.next(static_cast::_cnt_type>(m.sv.size())); stream.next(m.reserved0); for (std::size_t i = 0; i < m.sv.size(); ++i) { serialize(stream, m.sv[i]); @@ -2074,9 +2073,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSOL_ & m) { + ublox_msgs::msg::NavSOL_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.f_tow); @@ -2097,13 +2096,13 @@ struct UbloxSerializer > { stream.next(m.reserved2); } - inline static uint32_t serializedLength(const ublox_msgs::NavSOL_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSOL_ & m) { (void)m; return 52; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSOL_ & m) { + const ublox_msgs::msg::NavSOL_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.f_tow); @@ -2126,9 +2125,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSTATUS_ & m) { + ublox_msgs::msg::NavSTATUS_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.gps_fix); @@ -2139,13 +2138,13 @@ struct UbloxSerializer > { stream.next(m.msss); } - inline static uint32_t serializedLength(const ublox_msgs::NavSTATUS_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSTATUS_ & m) { (void)m; return 16; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSTATUS_ & m) { + const ublox_msgs::msg::NavSTATUS_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.gps_fix); @@ -2158,9 +2157,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSVIN_ & m) { + ublox_msgs::msg::NavSVIN_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.reserved0[0]); @@ -2183,13 +2182,13 @@ struct UbloxSerializer > { stream.next(m.reserved3[1]); } - inline static uint32_t serializedLength(const ublox_msgs::NavSVIN_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVIN_ & m) { (void)m; return 40; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSVIN_ & m) { + const ublox_msgs::msg::NavSVIN_ & m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.reserved0[0]); @@ -2214,8 +2213,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::NavSVINFOSV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavSVINFOSV_ & m) { stream.next(m.chn); stream.next(m.svid); stream.next(m.flags); @@ -2226,12 +2225,12 @@ struct UbloxSerializer > { stream.next(m.pr_res); } - inline static uint32_t serializedLength(const ublox_msgs::NavSVINFOSV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVINFOSV_ & m) { (void)m; return 12; } - inline static void write(UbloxOStream& stream, const ublox_msgs::NavSVINFOSV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::NavSVINFOSV_ & m) { stream.next(m.chn); stream.next(m.svid); stream.next(m.flags); @@ -2247,9 +2246,9 @@ struct UbloxSerializer > { /// @brief Serializes the NavSVINFO message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavSVINFO_ &m) { + ublox_msgs::msg::NavSVINFO_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.num_ch); @@ -2261,18 +2260,18 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::NavSVINFO_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavSVINFO_ &m) { return 8 + 12 * m.num_ch; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavSVINFO_ &m) { + const ublox_msgs::msg::NavSVINFO_ &m) { if (m.sv.size() != m.num_ch) { - ROS_ERROR("NavSVINFO num_ch must equal sv size"); + // ROS_ERROR("NavSVINFO num_ch must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.i_tow); - stream.next(static_cast::_num_ch_type>(m.sv.size())); + stream.next(static_cast::_num_ch_type>(m.sv.size())); stream.next(m.global_flags); stream.next(m.reserved2); for (std::size_t i = 0; i < m.sv.size(); ++i) { @@ -2282,9 +2281,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::NavVELNED_ & m) { + ublox_msgs::msg::NavVELNED_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.vel_n); @@ -2297,13 +2296,13 @@ struct UbloxSerializer > { stream.next(m.c_acc); } - inline static uint32_t serializedLength(const ublox_msgs::NavVELNED_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::NavVELNED_ & m) { (void)m; return 36; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::NavVELNED_ & m) { + const ublox_msgs::msg::NavVELNED_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.vel_n); @@ -2321,16 +2320,16 @@ struct UbloxSerializer > { /// @brief Serializes the RxmALM message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmALM_ &m) { + ublox_msgs::msg::RxmALM_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.week); m.dwrd.clear(); if (count == 40) { - typename ublox_msgs::RxmALM_::_dwrd_type::value_type temp; + typename ublox_msgs::msg::RxmALM_::_dwrd_type::value_type temp; m.dwrd.resize(8); for (std::size_t i = 0; i < 8; ++i) { stream.next(temp); @@ -2339,12 +2338,12 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::RxmALM_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmALM_ &m) { return 8 + (4 * m.dwrd.size()); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmALM_ &m) { + const ublox_msgs::msg::RxmALM_ &m) { UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.week); @@ -2358,10 +2357,10 @@ struct UbloxSerializer > { /// @brief Serializes the RxmEPH message which has a repeated block. /// template -struct UbloxSerializer > +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmEPH_ &m) { + ublox_msgs::msg::RxmEPH_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.svid); stream.next(m.how); @@ -2370,9 +2369,9 @@ struct UbloxSerializer > m.sf3d.clear(); if (count == 104) { - typename ublox_msgs::RxmEPH_::_sf1d_type::value_type temp1; - typename ublox_msgs::RxmEPH_::_sf2d_type::value_type temp2; - typename ublox_msgs::RxmEPH_::_sf3d_type::value_type temp3; + typename ublox_msgs::msg::RxmEPH_::_sf1d_type::value_type temp1; + typename ublox_msgs::msg::RxmEPH_::_sf2d_type::value_type temp2; + typename ublox_msgs::msg::RxmEPH_::_sf3d_type::value_type temp3; m.sf1d.resize(8); for (std::size_t i = 0; i < 8; ++i) { @@ -2392,30 +2391,30 @@ struct UbloxSerializer > } } - inline static uint32_t serializedLength(const ublox_msgs::RxmEPH_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmEPH_ &m) { return 8 + (4 * m.sf1d.size()) + (4 * m.sf2d.size()) + (4 * m.sf3d.size()); } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmEPH_ &m) { + const ublox_msgs::msg::RxmEPH_ &m) { UbloxOStream stream(data, size); stream.next(m.svid); stream.next(m.how); - for(std::size_t i = 0; i < m.sf1d.size(); ++i) { + for (std::size_t i = 0; i < m.sf1d.size(); ++i) { serialize(stream, m.sf1d[i]); } - for(std::size_t i = 0; i < m.sf2d.size(); ++i) { + for (std::size_t i = 0; i < m.sf2d.size(); ++i) { serialize(stream, m.sf2d[i]); } - for(std::size_t i = 0; i < m.sf3d.size(); ++i) { + for (std::size_t i = 0; i < m.sf3d.size(); ++i) { serialize(stream, m.sf3d[i]); } } }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::RxmRAWSV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmRAWSV_ & m) { stream.next(m.cp_mes); stream.next(m.pr_mes); stream.next(m.do_mes); @@ -2425,12 +2424,12 @@ struct UbloxSerializer > { stream.next(m.lli); } - inline static uint32_t serializedLength(const ublox_msgs::RxmRAWSV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWSV_ & m) { (void)m; return 24; } - inline static void write(UbloxOStream& stream, const ublox_msgs::RxmRAWSV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmRAWSV_ & m) { stream.next(m.cp_mes); stream.next(m.pr_mes); stream.next(m.do_mes); @@ -2445,9 +2444,9 @@ struct UbloxSerializer > { /// @brief Serializes the RxmRAW message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmRAW_ &m) { + ublox_msgs::msg::RxmRAW_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.rcv_tow); stream.next(m.week); @@ -2459,19 +2458,19 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::RxmRAW_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAW_ &m) { return 8 + 24 * m.num_sv; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmRAW_ &m) { + const ublox_msgs::msg::RxmRAW_ &m) { if (m.sv.size() != m.num_sv) { - ROS_ERROR("RxmRAW num_sv must equal sv size"); + // ROS_ERROR("RxmRAW num_sv must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.rcv_tow); stream.next(m.week); - stream.next(static_cast::_num_sv_type>(m.sv.size())); + stream.next(static_cast::_num_sv_type>(m.sv.size())); stream.next(m.reserved1); for (std::size_t i = 0; i < m.sv.size(); ++i) { serialize(stream, m.sv[i]); @@ -2480,8 +2479,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::RxmRAWXMeas_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmRAWXMeas_ & m) { stream.next(m.pr_mes); stream.next(m.cp_mes); stream.next(m.do_mes); @@ -2498,12 +2497,12 @@ struct UbloxSerializer > { stream.next(m.reserved1); } - inline static uint32_t serializedLength(const ublox_msgs::RxmRAWXMeas_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWXMeas_ & m) { (void)m; return 32; } - inline static void write(UbloxOStream& stream, const ublox_msgs::RxmRAWXMeas_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmRAWXMeas_ & m) { stream.next(m.pr_mes); stream.next(m.cp_mes); stream.next(m.do_mes); @@ -2525,9 +2524,9 @@ struct UbloxSerializer > { /// @brief Serializes the RxmRAWX message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmRAWX_ &m) { + ublox_msgs::msg::RxmRAWX_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.rcv_tow); stream.next(m.week); @@ -2542,20 +2541,20 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::RxmRAWX_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRAWX_ &m) { return 16 + 32 * m.num_meas; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmRAWX_ &m) { + const ublox_msgs::msg::RxmRAWX_ &m) { if (m.meas.size() != m.num_meas) { - ROS_ERROR("RxmRAWX num_meas must equal meas size"); + // ROS_ERROR("RxmRAWX num_meas must equal meas size"); } UbloxOStream stream(data, size); stream.next(m.rcv_tow); stream.next(m.week); stream.next(m.leap_s); - stream.next(static_cast::_num_meas_type>(m.meas.size())); + stream.next(static_cast::_num_meas_type>(m.meas.size())); stream.next(m.rec_stat); stream.next(m.version); stream.next(m.reserved1); @@ -2566,9 +2565,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmRTCM_ & m) { + ublox_msgs::msg::RxmRTCM_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.version); stream.next(m.flags); @@ -2578,13 +2577,13 @@ struct UbloxSerializer > { stream.next(m.msg_type); } - inline static uint32_t serializedLength(const ublox_msgs::RxmRTCM_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmRTCM_ & m) { (void)m; return 8; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmRTCM_ & m) { + const ublox_msgs::msg::RxmRTCM_ & m) { UbloxOStream stream(data, size); stream.next(m.version); stream.next(m.flags); @@ -2596,9 +2595,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmSFRB_ & m) { + ublox_msgs::msg::RxmSFRB_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.chn); stream.next(m.svid); @@ -2614,13 +2613,13 @@ struct UbloxSerializer > { stream.next(m.dwrd[9]); } - inline static uint32_t serializedLength(const ublox_msgs::RxmSFRB_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSFRB_ & m) { (void)m; return 42; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmSFRB_ & m) { + const ublox_msgs::msg::RxmSFRB_ & m) { UbloxOStream stream(data, size); stream.next(m.chn); stream.next(m.svid); @@ -2641,9 +2640,9 @@ struct UbloxSerializer > { /// @brief Serializes the RxmSFRBX message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmSFRBX_ &m) { + ublox_msgs::msg::RxmSFRBX_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.gnss_id); stream.next(m.sv_id); @@ -2659,21 +2658,21 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::RxmSFRBX_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSFRBX_ &m) { return 8 + 4 * m.num_words; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmSFRBX_ &m) { + const ublox_msgs::msg::RxmSFRBX_ &m) { if (m.dwrd.size() != m.num_words) { - ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); + // ROS_ERROR("RxmSFRBX num_words must equal dwrd size"); } UbloxOStream stream(data, size); stream.next(m.gnss_id); stream.next(m.sv_id); stream.next(m.reserved0); stream.next(m.freq_id); - stream.next(static_cast::_num_words_type>(m.dwrd.size())); + stream.next(static_cast::_num_words_type>(m.dwrd.size())); stream.next(m.chn); stream.next(m.version); stream.next(m.reserved1); @@ -2684,8 +2683,8 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { - inline static void read(UbloxIStream& stream, ublox_msgs::RxmSVSISV_ & m) { +struct UbloxSerializer > { + inline static void read(UbloxIStream& stream, ublox_msgs::msg::RxmSVSISV_ & m) { stream.next(m.svid); stream.next(m.sv_flag); stream.next(m.azim); @@ -2693,12 +2692,12 @@ struct UbloxSerializer > { stream.next(m.age); } - inline static uint32_t serializedLength(const ublox_msgs::RxmSVSISV_ & m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSVSISV_ & m) { (void)m; return 6; } - inline static void write(UbloxOStream& stream, const ublox_msgs::RxmSVSISV_ & m) { + inline static void write(UbloxOStream& stream, const ublox_msgs::msg::RxmSVSISV_ & m) { stream.next(m.svid); stream.next(m.sv_flag); stream.next(m.azim); @@ -2711,9 +2710,9 @@ struct UbloxSerializer > { /// @brief Serializes the RxmSVSI message which has a repeated block. /// template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::RxmSVSI_ &m) { + ublox_msgs::msg::RxmSVSI_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.week); @@ -2725,20 +2724,20 @@ struct UbloxSerializer > { } } - inline static uint32_t serializedLength(const ublox_msgs::RxmSVSI_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::RxmSVSI_ &m) { return 8 + 6 * m.num_sv; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::RxmSVSI_ &m) { + const ublox_msgs::msg::RxmSVSI_ &m) { if (m.sv.size() != m.num_sv) { - ROS_ERROR("RxmSVSI num_sv must equal sv size"); + // ROS_ERROR("RxmSVSI num_sv must equal sv size"); } UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.week); stream.next(m.num_vis); - stream.next(static_cast::_num_sv_type>(m.sv.size())); + stream.next(static_cast::_num_sv_type>(m.sv.size())); for (std::size_t i = 0; i < m.sv.size(); ++i) { serialize(stream, m.sv[i]); } @@ -2746,9 +2745,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::TimTM2_ &m) { + ublox_msgs::msg::TimTM2_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.ch); stream.next(m.flags); @@ -2762,13 +2761,13 @@ struct UbloxSerializer > { stream.next(m.acc_est); } - inline static uint32_t serializedLength(const ublox_msgs::TimTM2_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::TimTM2_ &m) { (void)m; return 28; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::TimTM2_ &m) { + const ublox_msgs::msg::TimTM2_ &m) { UbloxOStream stream(data, size); stream.next(m.ch); stream.next(m.flags); @@ -2784,9 +2783,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::UpdSOS_ &m) { + ublox_msgs::msg::UpdSOS_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.cmd); stream.next(m.reserved1[0]); @@ -2794,13 +2793,13 @@ struct UbloxSerializer > { stream.next(m.reserved1[2]); } - inline static uint32_t serializedLength(const ublox_msgs::UpdSOS_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::UpdSOS_ &m) { (void)m; return 4; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::UpdSOS_ &m) { + const ublox_msgs::msg::UpdSOS_ &m) { UbloxOStream stream(data, size); stream.next(m.cmd); stream.next(m.reserved1[0]); @@ -2810,9 +2809,9 @@ struct UbloxSerializer > { }; template -struct UbloxSerializer > { +struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, - ublox_msgs::UpdSOSAck_ &m) { + ublox_msgs::msg::UpdSOSAck_ &m) { UbloxIStream stream(const_cast(data), count); stream.next(m.cmd); stream.next(m.reserved0[0]); @@ -2824,13 +2823,13 @@ struct UbloxSerializer > { stream.next(m.reserved1[2]); } - inline static uint32_t serializedLength(const ublox_msgs::UpdSOSAck_ &m) { + inline static uint32_t serializedLength(const ublox_msgs::msg::UpdSOSAck_ &m) { (void)m; return 8; } inline static void write(uint8_t *data, uint32_t size, - const ublox_msgs::UpdSOSAck_ &m) { + const ublox_msgs::msg::UpdSOSAck_ &m) { UbloxOStream stream(data, size); stream.next(m.cmd); stream.next(m.reserved0[0]); diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index 8602e222..91365de6 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -26,88 +26,88 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#ifndef UBLOX_MSGS_HPP -#define UBLOX_MSGS_HPP +#ifndef UBLOX_MSGS_UBLOX_MSGS_HPP +#define UBLOX_MSGS_UBLOX_MSGS_HPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include +#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include -#include +#include -#include +#include namespace ublox_msgs { @@ -152,36 +152,36 @@ namespace Class { namespace Message { namespace NAV { - static const uint8_t ATT = NavATT::MESSAGE_ID; - static const uint8_t CLOCK = NavCLOCK::MESSAGE_ID; - static const uint8_t DGPS = NavDGPS::MESSAGE_ID; - static const uint8_t DOP = NavDOP::MESSAGE_ID; - static const uint8_t POSECEF = NavPOSECEF::MESSAGE_ID; - static const uint8_t POSLLH = NavPOSLLH::MESSAGE_ID; - static const uint8_t RELPOSNED = NavRELPOSNED::MESSAGE_ID; - static const uint8_t RELPOSNED9 = NavRELPOSNED9::MESSAGE_ID; - static const uint8_t SBAS = NavSBAS::MESSAGE_ID; - static const uint8_t SOL = NavSOL::MESSAGE_ID; - static const uint8_t PVT = NavPVT::MESSAGE_ID; - static const uint8_t SAT = NavSAT::MESSAGE_ID; - static const uint8_t STATUS = NavSTATUS::MESSAGE_ID; - static const uint8_t SVINFO = NavSVINFO::MESSAGE_ID; - static const uint8_t SVIN = NavSVIN::MESSAGE_ID; - static const uint8_t TIMEGPS = NavTIMEGPS::MESSAGE_ID; - static const uint8_t TIMEUTC = NavTIMEUTC::MESSAGE_ID; - static const uint8_t VELECEF = NavVELECEF::MESSAGE_ID; - static const uint8_t VELNED = NavVELNED::MESSAGE_ID; + static const uint8_t ATT = ublox_msgs::msg::NavATT::MESSAGE_ID; + static const uint8_t CLOCK = ublox_msgs::msg::NavCLOCK::MESSAGE_ID; + static const uint8_t DGPS = ublox_msgs::msg::NavDGPS::MESSAGE_ID; + static const uint8_t DOP = ublox_msgs::msg::NavDOP::MESSAGE_ID; + static const uint8_t POSECEF = ublox_msgs::msg::NavPOSECEF::MESSAGE_ID; + static const uint8_t POSLLH = ublox_msgs::msg::NavPOSLLH::MESSAGE_ID; + static const uint8_t RELPOSNED = ublox_msgs::msg::NavRELPOSNED::MESSAGE_ID; + static const uint8_t RELPOSNED9 = ublox_msgs::msg::NavRELPOSNED9::MESSAGE_ID; + static const uint8_t SBAS = ublox_msgs::msg::NavSBAS::MESSAGE_ID; + static const uint8_t SOL = ublox_msgs::msg::NavSOL::MESSAGE_ID; + static const uint8_t PVT = ublox_msgs::msg::NavPVT::MESSAGE_ID; + static const uint8_t SAT = ublox_msgs::msg::NavSAT::MESSAGE_ID; + static const uint8_t STATUS = ublox_msgs::msg::NavSTATUS::MESSAGE_ID; + static const uint8_t SVINFO = ublox_msgs::msg::NavSVINFO::MESSAGE_ID; + static const uint8_t SVIN = ublox_msgs::msg::NavSVIN::MESSAGE_ID; + static const uint8_t TIMEGPS = ublox_msgs::msg::NavTIMEGPS::MESSAGE_ID; + static const uint8_t TIMEUTC = ublox_msgs::msg::NavTIMEUTC::MESSAGE_ID; + static const uint8_t VELECEF = ublox_msgs::msg::NavVELECEF::MESSAGE_ID; + static const uint8_t VELNED = ublox_msgs::msg::NavVELNED::MESSAGE_ID; } namespace RXM { - static const uint8_t ALM = RxmALM::MESSAGE_ID; - static const uint8_t EPH = RxmEPH::MESSAGE_ID; - static const uint8_t RAW = RxmRAW::MESSAGE_ID; - static const uint8_t RAWX = RxmRAWX::MESSAGE_ID; - static const uint8_t RTCM = RxmRTCM::MESSAGE_ID; - static const uint8_t SFRB = RxmSFRB::MESSAGE_ID; - static const uint8_t SFRBX = RxmSFRBX::MESSAGE_ID; - static const uint8_t SVSI = RxmSVSI::MESSAGE_ID; + static const uint8_t ALM = ublox_msgs::msg::RxmALM::MESSAGE_ID; + static const uint8_t EPH = ublox_msgs::msg::RxmEPH::MESSAGE_ID; + static const uint8_t RAW = ublox_msgs::msg::RxmRAW::MESSAGE_ID; + static const uint8_t RAWX = ublox_msgs::msg::RxmRAWX::MESSAGE_ID; + static const uint8_t RTCM = ublox_msgs::msg::RxmRTCM::MESSAGE_ID; + static const uint8_t SFRB = ublox_msgs::msg::RxmSFRB::MESSAGE_ID; + static const uint8_t SFRBX = ublox_msgs::msg::RxmSFRBX::MESSAGE_ID; + static const uint8_t SVSI = ublox_msgs::msg::RxmSVSI::MESSAGE_ID; } namespace INF { @@ -198,62 +198,62 @@ namespace Message { } namespace AID { - static const uint8_t ALM = AidALM::MESSAGE_ID; - static const uint8_t EPH = AidEPH::MESSAGE_ID; - static const uint8_t HUI = AidHUI::MESSAGE_ID; + static const uint8_t ALM = ublox_msgs::msg::AidALM::MESSAGE_ID; + static const uint8_t EPH = ublox_msgs::msg::AidEPH::MESSAGE_ID; + static const uint8_t HUI = ublox_msgs::msg::AidHUI::MESSAGE_ID; } namespace CFG { - static const uint8_t ANT = CfgANT::MESSAGE_ID; - static const uint8_t CFG = CfgCFG::MESSAGE_ID; - static const uint8_t DAT = CfgDAT::MESSAGE_ID; - static const uint8_t GNSS = CfgGNSS::MESSAGE_ID; - static const uint8_t HNR = CfgHNR::MESSAGE_ID; - static const uint8_t INF = CfgINF::MESSAGE_ID; - static const uint8_t DGNSS = CfgDGNSS::MESSAGE_ID; - static const uint8_t MSG = CfgMSG::MESSAGE_ID; - static const uint8_t NAV5 = CfgNAV5::MESSAGE_ID; - static const uint8_t NAVX5 = CfgNAVX5::MESSAGE_ID; - static const uint8_t NMEA = CfgNMEA::MESSAGE_ID; - static const uint8_t PRT = CfgPRT::MESSAGE_ID; - static const uint8_t RATE = CfgRATE::MESSAGE_ID; - static const uint8_t RST = CfgRST::MESSAGE_ID; - static const uint8_t SBAS = CfgSBAS::MESSAGE_ID; - static const uint8_t TMODE3 = CfgTMODE3::MESSAGE_ID; - static const uint8_t USB = CfgUSB::MESSAGE_ID; + static const uint8_t ANT = ublox_msgs::msg::CfgANT::MESSAGE_ID; + static const uint8_t CFG = ublox_msgs::msg::CfgCFG::MESSAGE_ID; + static const uint8_t DAT = ublox_msgs::msg::CfgDAT::MESSAGE_ID; + static const uint8_t GNSS = ublox_msgs::msg::CfgGNSS::MESSAGE_ID; + static const uint8_t HNR = ublox_msgs::msg::CfgHNR::MESSAGE_ID; + static const uint8_t INF = ublox_msgs::msg::CfgINF::MESSAGE_ID; + static const uint8_t DGNSS = ublox_msgs::msg::CfgDGNSS::MESSAGE_ID; + static const uint8_t MSG = ublox_msgs::msg::CfgMSG::MESSAGE_ID; + static const uint8_t NAV5 = ublox_msgs::msg::CfgNAV5::MESSAGE_ID; + static const uint8_t NAVX5 = ublox_msgs::msg::CfgNAVX5::MESSAGE_ID; + static const uint8_t NMEA = ublox_msgs::msg::CfgNMEA::MESSAGE_ID; + static const uint8_t PRT = ublox_msgs::msg::CfgPRT::MESSAGE_ID; + static const uint8_t RATE = ublox_msgs::msg::CfgRATE::MESSAGE_ID; + static const uint8_t RST = ublox_msgs::msg::CfgRST::MESSAGE_ID; + static const uint8_t SBAS = ublox_msgs::msg::CfgSBAS::MESSAGE_ID; + static const uint8_t TMODE3 = ublox_msgs::msg::CfgTMODE3::MESSAGE_ID; + static const uint8_t USB = ublox_msgs::msg::CfgUSB::MESSAGE_ID; } namespace UPD { //! SOS and SOS_Ack have the same message ID, but different lengths - static const uint8_t SOS = UpdSOS::MESSAGE_ID; + static const uint8_t SOS = ublox_msgs::msg::UpdSOS::MESSAGE_ID; } namespace MON { - static const uint8_t GNSS = MonGNSS::MESSAGE_ID; - static const uint8_t HW = MonHW::MESSAGE_ID; - static const uint8_t VER = MonVER::MESSAGE_ID; + static const uint8_t GNSS = ublox_msgs::msg::MonGNSS::MESSAGE_ID; + static const uint8_t HW = ublox_msgs::msg::MonHW::MESSAGE_ID; + static const uint8_t VER = ublox_msgs::msg::MonVER::MESSAGE_ID; } namespace ESF { - static const uint8_t INS = EsfINS::MESSAGE_ID; - static const uint8_t MEAS = EsfMEAS::MESSAGE_ID; - static const uint8_t RAW = EsfRAW::MESSAGE_ID; - static const uint8_t STATUS = EsfSTATUS::MESSAGE_ID; + static const uint8_t INS = ublox_msgs::msg::EsfINS::MESSAGE_ID; + static const uint8_t MEAS = ublox_msgs::msg::EsfMEAS::MESSAGE_ID; + static const uint8_t RAW = ublox_msgs::msg::EsfRAW::MESSAGE_ID; + static const uint8_t STATUS = ublox_msgs::msg::EsfSTATUS::MESSAGE_ID; } namespace MGA { - static const uint8_t GAL = MgaGAL::MESSAGE_ID; + static const uint8_t GAL = ublox_msgs::msg::MgaGAL::MESSAGE_ID; } namespace HNR { - static const uint8_t PVT = HnrPVT::MESSAGE_ID; + static const uint8_t PVT = ublox_msgs::msg::HnrPVT::MESSAGE_ID; } namespace TIM { - static const uint8_t TM2 = TimTM2::MESSAGE_ID; + static const uint8_t TM2 = ublox_msgs::msg::TimTM2::MESSAGE_ID; } } } //!< namespace ublox_msgs -#endif //!< UBLOX_MSGS_HPP +#endif //!< UBLOX_MSGS_UBLOX_MSGS_HPP diff --git a/ublox_msgs/package.xml b/ublox_msgs/package.xml index 124cd2c2..89bdce08 100644 --- a/ublox_msgs/package.xml +++ b/ublox_msgs/package.xml @@ -1,4 +1,6 @@ - + + + ublox_msgs 1.2.0 @@ -11,22 +13,18 @@ BSD http://ros.org/wiki/ublox - catkin + ament_cmake - message_generation - message_runtime + rosidl_default_generators - ublox_serialization - ublox_serialization + std_msgs + sensor_msgs + ublox_serialization - std_msgs - std_msgs - - sensor_msgs - sensor_msgs + rosidl_interface_packages - + ament_cmake diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index c83153c8..cb2f47d1 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -35,169 +35,169 @@ template std::vector > ublox::Message::keys_; DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, - ublox_msgs, NavATT); + ublox_msgs, NavATT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, - ublox_msgs, NavCLOCK); + ublox_msgs, NavCLOCK) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, - ublox_msgs, NavDGPS); + ublox_msgs, NavDGPS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, - ublox_msgs, NavDOP); + ublox_msgs, NavDOP) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, - ublox_msgs, NavPOSECEF); + ublox_msgs, NavPOSECEF) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH, - ublox_msgs, NavPOSLLH); + ublox_msgs, NavPOSLLH) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED, ublox_msgs, - NavRELPOSNED); + NavRELPOSNED) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::RELPOSNED9, ublox_msgs, - NavRELPOSNED9); + NavRELPOSNED9) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SBAS, - ublox_msgs, NavSBAS); + ublox_msgs, NavSBAS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SOL, - ublox_msgs, NavSOL); + ublox_msgs, NavSOL) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT); + ublox_msgs, NavPVT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::PVT, - ublox_msgs, NavPVT7); + ublox_msgs, NavPVT7) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SAT, - ublox_msgs, NavSAT); + ublox_msgs, NavSAT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::STATUS, - ublox_msgs, NavSTATUS); + ublox_msgs, NavSTATUS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVIN, - ublox_msgs, NavSVIN); + ublox_msgs, NavSVIN) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::SVINFO, - ublox_msgs, NavSVINFO); + ublox_msgs, NavSVINFO) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEGPS, - ublox_msgs, NavTIMEGPS); + ublox_msgs, NavTIMEGPS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::TIMEUTC, - ublox_msgs, NavTIMEUTC); + ublox_msgs, NavTIMEUTC) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELECEF, - ublox_msgs, NavVELECEF); + ublox_msgs, NavVELECEF) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::VELNED, - ublox_msgs, NavVELNED); + ublox_msgs, NavVELNED) // ACK messages are declared differently because they both have the same // protocol, so only 1 ROS message is used DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::NACK, - ublox_msgs, Ack); + ublox_msgs, Ack) DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::ACK, ublox_msgs::Message::ACK::ACK, - ublox_msgs, Ack, ACK); + ublox_msgs, Ack, ACK) // INF messages are declared differently because they all have the same // protocol, so only 1 ROS message is used. DECLARE_UBLOX_MESSAGE can only // be called once, and DECLARE_UBLOX_MESSAGE_ID is called for the following // messages DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::INF, ublox_msgs::Message::INF::ERROR, - ublox_msgs, Inf); + ublox_msgs, Inf) DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::WARNING, - ublox_msgs, Inf, WARNING); + ublox_msgs, Inf, WARNING) DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::NOTICE, - ublox_msgs, Inf, NOTICE); + ublox_msgs, Inf, NOTICE) DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::TEST, - ublox_msgs, Inf, TEST); + ublox_msgs, Inf, TEST) DECLARE_UBLOX_MESSAGE_ID(ublox_msgs::Class::INF, ublox_msgs::Message::INF::DEBUG, - ublox_msgs, Inf, DEBUG); + ublox_msgs, Inf, DEBUG) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::ALM, - ublox_msgs, RxmALM); + ublox_msgs, RxmALM) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::EPH, - ublox_msgs, RxmEPH); + ublox_msgs, RxmEPH) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAW, - ublox_msgs, RxmRAW); + ublox_msgs, RxmRAW) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RAWX, - ublox_msgs, RxmRAWX); + ublox_msgs, RxmRAWX) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::RTCM, - ublox_msgs, RxmRTCM); + ublox_msgs, RxmRTCM) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRB, - ublox_msgs, RxmSFRB); + ublox_msgs, RxmSFRB) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SFRBX, - ublox_msgs, RxmSFRBX); + ublox_msgs, RxmSFRBX) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::RXM, ublox_msgs::Message::RXM::SVSI, - ublox_msgs, RxmSVSI); + ublox_msgs, RxmSVSI) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::ANT, - ublox_msgs, CfgANT); + ublox_msgs, CfgANT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::CFG, - ublox_msgs, CfgCFG); + ublox_msgs, CfgCFG) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DAT, - ublox_msgs, CfgDAT); + ublox_msgs, CfgDAT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::DGNSS, - ublox_msgs, CfgDGNSS); + ublox_msgs, CfgDGNSS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::GNSS, - ublox_msgs, CfgGNSS); + ublox_msgs, CfgGNSS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::HNR, - ublox_msgs, CfgHNR); + ublox_msgs, CfgHNR) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::INF, - ublox_msgs, CfgINF); + ublox_msgs, CfgINF) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::MSG, - ublox_msgs, CfgMSG); + ublox_msgs, CfgMSG) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAV5, - ublox_msgs, CfgNAV5); + ublox_msgs, CfgNAV5) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NAVX5, - ublox_msgs, CfgNAVX5); + ublox_msgs, CfgNAVX5) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA); + ublox_msgs, CfgNMEA) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA6); + ublox_msgs, CfgNMEA6) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::NMEA, - ublox_msgs, CfgNMEA7); + ublox_msgs, CfgNMEA7) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::PRT, - ublox_msgs, CfgPRT); + ublox_msgs, CfgPRT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RATE, - ublox_msgs, CfgRATE); + ublox_msgs, CfgRATE) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::RST, - ublox_msgs, CfgRST); + ublox_msgs, CfgRST) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, - ublox_msgs, CfgTMODE3); + ublox_msgs, CfgTMODE3) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, - ublox_msgs, CfgUSB); + ublox_msgs, CfgUSB) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOS); + ublox_msgs, UpdSOS) // SOS and SOS_Ack have the same message ID, but different lengths DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, - ublox_msgs, UpdSOSAck); + ublox_msgs, UpdSOSAck) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::GNSS, - ublox_msgs, MonGNSS); + ublox_msgs, MonGNSS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW); + ublox_msgs, MonHW) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::HW, - ublox_msgs, MonHW6); + ublox_msgs, MonHW6) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER, - ublox_msgs, MonVER); + ublox_msgs, MonVER) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, - ublox_msgs, AidALM); + ublox_msgs, AidALM) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, - ublox_msgs, AidEPH); + ublox_msgs, AidEPH) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, - ublox_msgs, AidHUI); + ublox_msgs, AidHUI) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS, - ublox_msgs, EsfINS); + ublox_msgs, EsfINS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS, - ublox_msgs, EsfMEAS); + ublox_msgs, EsfMEAS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::RAW, - ublox_msgs, EsfRAW); + ublox_msgs, EsfRAW) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::STATUS, - ublox_msgs, EsfSTATUS); + ublox_msgs, EsfSTATUS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::MGA, ublox_msgs::Message::MGA::GAL, - ublox_msgs, MgaGAL); + ublox_msgs, MgaGAL) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::HNR, ublox_msgs::Message::HNR::PVT, - ublox_msgs, HnrPVT); + ublox_msgs, HnrPVT) // TIM messages DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::TIM, ublox_msgs::Message::TIM::TM2, - ublox_msgs, TimTM2); + ublox_msgs, TimTM2) From 1eabc8a1814c94d690b5b75d5a28c061b3732d87 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 19:06:56 +0000 Subject: [PATCH 084/152] Port ublox_gps to ROS 2. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 89 ++-- ublox_gps/COLCON_IGNORE | 0 .../include/ublox_gps/adr_udr_product.hpp | 50 +- ublox_gps/include/ublox_gps/async_worker.hpp | 22 +- ublox_gps/include/ublox_gps/callback.hpp | 26 +- .../include/ublox_gps/fix_diagnostic.hpp | 4 +- ublox_gps/include/ublox_gps/fts_product.hpp | 6 +- ublox_gps/include/ublox_gps/gps.hpp | 18 +- .../include/ublox_gps/hp_pos_rec_product.hpp | 20 +- .../include/ublox_gps/hpg_ref_product.hpp | 18 +- .../include/ublox_gps/hpg_rov_product.hpp | 16 +- ublox_gps/include/ublox_gps/node.hpp | 39 +- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 21 +- .../include/ublox_gps/raw_data_product.hpp | 21 +- ublox_gps/include/ublox_gps/tim_product.hpp | 26 +- .../include/ublox_gps/ublox_firmware.hpp | 8 +- .../include/ublox_gps/ublox_firmware6.hpp | 52 +- .../include/ublox_gps/ublox_firmware7.hpp | 28 +- .../include/ublox_gps/ublox_firmware7plus.hpp | 76 +-- .../include/ublox_gps/ublox_firmware8.hpp | 34 +- .../include/ublox_gps/ublox_firmware9.hpp | 6 +- .../ublox_gps/ublox_topic_diagnostic.hpp | 6 +- ublox_gps/include/ublox_gps/utils.hpp | 54 +- ublox_gps/package.xml | 18 +- ublox_gps/src/adr_udr_product.cpp | 102 ++-- ublox_gps/src/gps.cpp | 206 ++++---- ublox_gps/src/hp_pos_rec_product.cpp | 33 +- ublox_gps/src/hpg_ref_product.cpp | 82 +-- ublox_gps/src/hpg_rov_product.cpp | 32 +- ublox_gps/src/logger_node_pa.cpp | 16 +- ublox_gps/src/node.cpp | 496 +++++++++--------- ublox_gps/src/node_main.cpp | 16 +- ublox_gps/src/raw_data_pa.cpp | 54 +- ublox_gps/src/raw_data_product.cpp | 46 +- ublox_gps/src/tim_product.cpp | 47 +- ublox_gps/src/ublox_firmware.cpp | 6 +- ublox_gps/src/ublox_firmware6.cpp | 150 +++--- ublox_gps/src/ublox_firmware7.cpp | 138 ++--- ublox_gps/src/ublox_firmware8.cpp | 168 +++--- ublox_gps/src/ublox_firmware9.cpp | 6 +- 40 files changed, 1143 insertions(+), 1113 deletions(-) delete mode 100644 ublox_gps/COLCON_IGNORE diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 82747ec5..5e5b54a0 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -1,69 +1,72 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox_gps) -find_package(catkin REQUIRED COMPONENTS - roscpp - roscpp_serialization - tf2 - ublox_msgs - ublox_serialization - diagnostic_updater -) -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS tf2 roscpp ublox_msgs ublox_serialization) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -# include other ublox packages -include_directories(${PROJECT_SOURCE_DIR}/include) -include_directories(${catkin_INCLUDE_DIRS}) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") + +find_package(ament_cmake_ros REQUIRED) find_package(asio REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ublox_msgs REQUIRED) +find_package(ublox_serialization REQUIRED) -# link pthread -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread") +include_directories(include) # build library add_library(ublox_gps src/gps.cpp) - -# fix msg compile order bug -add_dependencies(ublox_gps ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(ublox_gps - ${catkin_LIBRARIES} +ament_target_dependencies(ublox_gps + "asio" + "ublox_msgs" + "ublox_serialization" ) # build node add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp src/hp_pos_rec_product.cpp src/node_main.cpp) -set_target_properties(ublox_gps_node PROPERTIES OUTPUT_NAME ublox_gps) - -target_link_libraries(ublox_gps_node ${catkin_LIBRARIES}) +ament_target_dependencies(ublox_gps_node + "diagnostic_msgs" + "diagnostic_updater" + "geometry_msgs" + "rclcpp" + "sensor_msgs" + "std_msgs" + "ublox_msgs" +) target_link_libraries(ublox_gps_node ublox_gps) # build logger node add_executable(ublox_logger_node src/logger_node_pa.cpp src/raw_data_pa.cpp) set_target_properties(ublox_logger_node PROPERTIES OUTPUT_NAME ublox_logger) - -target_link_libraries(ublox_logger_node ${catkin_LIBRARIES}) - -install(TARGETS ublox_gps ublox_gps_node ublox_logger_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_target_dependencies(ublox_logger_node + "rclcpp" + "std_msgs" ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE +install(TARGETS ublox_gps + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config - PATTERN ".svn" EXCLUDE +install(TARGETS + ublox_gps_node ublox_logger_node + DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE +install(DIRECTORY include/ + DESTINATION include ) + +ament_package() diff --git a/ublox_gps/COLCON_IGNORE b/ublox_gps/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 0c37460f..4952a7c8 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -4,13 +4,18 @@ #include #include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include #include #include @@ -23,7 +28,7 @@ namespace ublox_node { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -44,8 +49,8 @@ class AdrUdrProduct final : public virtual ComponentInterface { * @todo unimplemented */ void initializeRosDiagnostics() override { - ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", - "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); + // RCLCPP_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", + // "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); } /** @@ -60,27 +65,26 @@ class AdrUdrProduct final : public virtual ComponentInterface { //! Whether or not to enable dead reckoning bool use_adr_; - sensor_msgs::Imu imu_; - sensor_msgs::TimeReference t_ref_; - ublox_msgs::TimTM2 timtm2; + sensor_msgs::msg::Imu imu_; + sensor_msgs::msg::TimeReference t_ref_; - void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m); + void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); - ros::Publisher imu_pub_; - ros::Publisher time_ref_pub_; - ros::Publisher nav_att_pub_; - ros::Publisher esf_ins_pub_; - ros::Publisher esf_meas_pub_; - ros::Publisher esf_raw_pub_; - ros::Publisher esf_status_pub_; - ros::Publisher hnr_pvt_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr time_ref_pub_; + rclcpp::Publisher::SharedPtr nav_att_pub_; + rclcpp::Publisher::SharedPtr esf_ins_pub_; + rclcpp::Publisher::SharedPtr esf_meas_pub_; + rclcpp::Publisher::SharedPtr esf_raw_pub_; + rclcpp::Publisher::SharedPtr esf_status_pub_; + rclcpp::Publisher::SharedPtr hnr_pvt_pub_; uint16_t nav_rate_; uint16_t meas_rate_; std::string frame_id_; std::shared_ptr updater_; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index e4933b7b..bb7315b4 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -134,7 +134,7 @@ class AsyncWorker final : public Worker { bool stopping_; //!< Whether or not the I/O service is closed - bool debug_; //!< Used to determine which debug messages to display + int debug_; //!< Used to determine which debug messages to display }; template @@ -166,12 +166,12 @@ bool AsyncWorker::send(const unsigned char* data, const unsigned int size) { std::lock_guard lock(write_mutex_); if (size == 0) { - ROS_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); + // RCLCPP_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); return true; } if (out_.capacity() - out_.size() < size) { - ROS_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); + // RCLCPP_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); return false; } out_.insert(out_.end(), data, data + size); @@ -197,7 +197,7 @@ void AsyncWorker::doWrite() { it != out_.end(); ++it) { oss << std::hex << static_cast(*it) << " "; } - ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); + // RCLCPP_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } // Clear the buffer & unlock out_.clear(); @@ -219,9 +219,9 @@ void AsyncWorker::readEnd(const asio::error_code& error, std::size_t bytes_transfered) { std::lock_guard lock(read_mutex_); if (error) { - ROS_ERROR("U-Blox ASIO input buffer read error: %s, %li", - error.message().c_str(), - bytes_transfered); + // RCLCPP_ERROR("U-Blox ASIO input buffer read error: %s, %li", + // error.message().c_str(), + // bytes_transfered); } else if (bytes_transfered > 0) { in_buffer_size_ += bytes_transfered; @@ -239,8 +239,8 @@ void AsyncWorker::readEnd(const asio::error_code& error, it != in_.begin() + in_buffer_size_; ++it) { oss << std::hex << static_cast(*it) << " "; } - ROS_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - oss.str().c_str()); + // RCLCPP_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, + // oss.str().c_str()); } if (read_callback_) { @@ -262,8 +262,8 @@ void AsyncWorker::doClose() { asio::error_code error; stream_->close(error); if (error) { - ROS_ERROR_STREAM( - "Error while closing the AsyncWorker stream: " << error.message()); + // RCLCPP_ERROR_STREAM( + // "Error while closing the AsyncWorker stream: " << error.message()); } } diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 46c8e4ac..119ab0c1 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -37,8 +37,6 @@ #include #include -#include - #include namespace ublox_gps { @@ -94,20 +92,20 @@ class CallbackHandler_ final : public CallbackHandler { std::lock_guard lock(mutex_); try { if (!reader.read(message_)) { - ROS_DEBUG_COND(debug_ >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", - static_cast(reader.classId()), - static_cast(reader.messageId()), - reader.length()); + // RCLCPP_DEBUG_COND(debug_ >= 2, + // "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + // static_cast(reader.classId()), + // static_cast(reader.messageId()), + // reader.length()); condition_.notify_all(); return; } } catch (std::runtime_error& e) { - ROS_DEBUG_COND(debug_ >= 2, - "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", - static_cast(reader.classId()), - static_cast(reader.messageId()), - reader.length()); + // RCLCPP_DEBUG_COND(debug_ >= 2, + // "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", + // static_cast(reader.classId()), + // static_cast(reader.messageId()), + // reader.length()); condition_.notify_all(); return; } @@ -223,8 +221,8 @@ class CallbackHandlers final { it != reader.pos() + reader.length() + 8; ++it) { oss << std::hex << static_cast(*it) << " "; } - ROS_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, - oss.str().c_str()); + // RCLCPP_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, + // oss.str().c_str()); } handle(reader); diff --git a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp index 2a8c0f61..f9b2a4af 100644 --- a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp @@ -4,8 +4,8 @@ #include #include -#include -#include +#include +#include namespace ublox_node { diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp index d279b748..d66acbbd 100644 --- a/ublox_gps/include/ublox_gps/fts_product.hpp +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -32,8 +32,6 @@ #include -#include - #include #include @@ -49,8 +47,8 @@ class FtsProduct final : public virtual ComponentInterface { * @todo Currently unimplemented. */ void getRosParams() override { - ROS_WARN("Functionality specific to u-blox FTS devices is %s", - "unimplemented. See FtsProduct class in node.hpp & node.cpp."); + // RCLCPP_WARN("Functionality specific to u-blox FTS devices is %s", + // "unimplemented. See FtsProduct class in node.hpp & node.cpp."); } /** diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 86751f2d..5743464d 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -36,8 +36,6 @@ #include #include #include -// ROS -#include // Other u-blox packages #include // u-blox gps @@ -142,7 +140,7 @@ class Gps final { * @return true if the GNSS was configured, the device was reset, and the * I/O reset successfully */ - bool configGnss(ublox_msgs::CfgGNSS gnss, + bool configGnss(ublox_msgs::msg::CfgGNSS gnss, const std::chrono::milliseconds& wait); /** @@ -169,7 +167,7 @@ class Gps final { * configuration parameters * @return true on ACK, false on other conditions. */ - bool disableUart1(ublox_msgs::CfgPRT& prev_cfg); + bool disableUart1(ublox_msgs::msg::CfgPRT& prev_cfg); /** * @brief Configure the USB Port. @@ -223,7 +221,7 @@ class Gps final { * @return true on ACK, false if settings are incorrect or on other conditions */ bool configTmode3Fixed(bool lla_flag, - std::vector arp_position, + std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc); @@ -433,19 +431,19 @@ class Gps final { * @brief Callback handler for UBX-ACK message. * @param m the message to process */ - void processAck(const ublox_msgs::Ack &m); + void processAck(const ublox_msgs::msg::Ack &m); /** * @brief Callback handler for UBX-NACK message. * @param m the message to process */ - void processNack(const ublox_msgs::Ack &m); + void processNack(const ublox_msgs::msg::Ack &m); /** * @brief Callback handler for UBX-UPD-SOS-ACK message. * @param m the message to process */ - void processUpdSosAck(const ublox_msgs::UpdSOSAck &m); + void processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m); /** * @brief Execute save on shutdown procedure. @@ -535,8 +533,8 @@ bool Gps::configure(const ConfigT& message, bool wait) { std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); if (!writer.write(message)) { - ROS_ERROR("Failed to encode config message 0x%02x / 0x%02x", - message.CLASS_ID, message.MESSAGE_ID); + // RCLCPP_ERROR("Failed to encode config message 0x%02x / 0x%02x", + // message.CLASS_ID, message.MESSAGE_ID); return false; } // Send the message to the device diff --git a/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp index 89a40943..94464cb4 100644 --- a/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp +++ b/ublox_gps/include/ublox_gps/hp_pos_rec_product.hpp @@ -5,11 +5,11 @@ #include #include -#include -#include -#include +#include +#include +#include -#include +#include #include #include @@ -19,7 +19,7 @@ namespace ublox_node { class HpPosRecProduct final : public virtual HpgRefProduct { public: - explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); + explicit HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node); /** * @brief Subscribe to Rover messages, such as NavRELPOSNED. @@ -33,15 +33,15 @@ class HpPosRecProduct final : public virtual HpgRefProduct { * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m); + void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m); - sensor_msgs::Imu imu_; + sensor_msgs::msg::Imu imu_; //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED9 last_rel_pos_; + ublox_msgs::msg::NavRELPOSNED9 last_rel_pos_; - ros::Publisher nav_relposned_pub_; - ros::Publisher imu_pub_; + rclcpp::Publisher::SharedPtr nav_relposned_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; std::string frame_id_; }; diff --git a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp index 81618781..45fc6a79 100644 --- a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp @@ -4,10 +4,10 @@ #include #include -#include -#include +#include +#include -#include +#include #include #include @@ -24,7 +24,7 @@ class HpgRefProduct: public virtual ComponentInterface { //! Default measurement period for HPG devices constexpr static uint16_t kDefaultMeasPeriod = 250; - explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node); + explicit HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node); /** * @brief Get the ROS parameters specific to the Reference Station @@ -67,7 +67,7 @@ class HpgRefProduct: public virtual ComponentInterface { * configured RTCM messages. Publish received Nav SVIN messages if enabled. * @param m the message to process */ - void callbackNavSvIn(const ublox_msgs::NavSVIN& m); + void callbackNavSvIn(const ublox_msgs::msg::NavSVIN& m); protected: /** @@ -86,7 +86,7 @@ class HpgRefProduct: public virtual ComponentInterface { bool setTimeMode(std::shared_ptr gps); //! The last received Nav SVIN message - ublox_msgs::NavSVIN last_nav_svin_; + ublox_msgs::msg::NavSVIN last_nav_svin_; //! TMODE3 to set, such as disabled, survey-in, fixed uint8_t tmode3_; @@ -97,7 +97,7 @@ class HpgRefProduct: public virtual ComponentInterface { bool lla_flag_; //! Antenna Reference Point Position [m] or [deg] /*! Used only for fixed mode */ - std::vector arp_position_; + std::vector arp_position_; //! Antenna Reference Point Position High Precision [0.1 mm] or [deg * 1e-9] /*! Used only for fixed mode */ std::vector arp_position_hp_; @@ -129,7 +129,7 @@ class HpgRefProduct: public virtual ComponentInterface { TIME //!< Time mode, after survey-in or after configuring fixed mode } mode_; - ros::Publisher navsvin_pub_; + rclcpp::Publisher::SharedPtr navsvin_pub_; uint16_t nav_rate_; uint16_t meas_rate_; @@ -137,7 +137,7 @@ class HpgRefProduct: public virtual ComponentInterface { std::vector rtcms_; std::shared_ptr gps_; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp index 9e6b7720..07d660c5 100644 --- a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp @@ -3,10 +3,10 @@ #include -#include -#include +#include +#include -#include +#include #include #include @@ -29,7 +29,7 @@ class HpgRovProduct final : public virtual ComponentInterface { //! Diagnostic updater: RTCM topic frequency window [num messages] constexpr static int kRtcmFreqWindow = 25; - explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node); + explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Get the ROS parameters specific to the Rover configuration. @@ -70,11 +70,11 @@ class HpgRovProduct final : public virtual ComponentInterface { * * @details Publish received NavRELPOSNED messages if enabled */ - void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m); + void callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m); //! Last relative position (used for diagnostic updater) - ublox_msgs::NavRELPOSNED last_rel_pos_; + ublox_msgs::msg::NavRELPOSNED last_rel_pos_; //! The DGNSS mode /*! see CfgDGNSS message for possible values */ @@ -83,11 +83,11 @@ class HpgRovProduct final : public virtual ComponentInterface { //! The RTCM topic frequency diagnostic updater UbloxTopicDiagnostic freq_rtcm_; - ros::Publisher nav_rel_pos_ned_pub_; + rclcpp::Publisher::SharedPtr nav_rel_pos_ned_pub_; uint16_t nav_rate_; std::shared_ptr updater_; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 8d9802d9..57c7394e 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -35,12 +35,12 @@ #include #include // ROS includes -#include -#include +#include +#include // U-Blox msgs nicludes -#include -#include -#include +#include +#include +#include // Ublox GPS includes #include #include @@ -78,7 +78,7 @@ namespace ublox_node { * The UbloxNode calls the public methods of ComponentInterface for each * element in the components vector. */ -class UbloxNode final { +class UbloxNode final : public rclcpp::Node { public: //! How long to wait during I/O reset [s] constexpr static int kResetWait = 10; @@ -125,7 +125,7 @@ class UbloxNode final { /** * @brief Print an INF message to the ROS console. */ - void printInf(const ublox_msgs::Inf &m, uint8_t id); + void printInf(const ublox_msgs::msg::Inf &m, uint8_t id); private: @@ -178,7 +178,7 @@ class UbloxNode final { * @brief Poll messages from the U-Blox device. * @param event a timer indicating how often to poll the messages */ - void pollMessages(const ros::TimerEvent& event); + void pollMessages(); /** * @brief Configure INF messages, call after subscribe. @@ -223,7 +223,7 @@ class UbloxNode final { //! The measurement rate in Hz double rate_; //! User-defined Datum - ublox_msgs::CfgDAT cfg_dat_; + ublox_msgs::msg::CfgDAT cfg_dat_; //! SBAS Usage parameter (see CfgSBAS message) uint8_t sbas_usage_; //! Max SBAS parameter (see CfgSBAS message) @@ -231,21 +231,21 @@ class UbloxNode final { //! Dead reckoning limit parameter uint8_t dr_limit_; //! Parameters to load from non-volatile memory during configuration - ublox_msgs::CfgCFG load_; + ublox_msgs::msg::CfgCFG load_; //! Parameters to save to non-volatile memory after configuration - ublox_msgs::CfgCFG save_; + ublox_msgs::msg::CfgCFG save_; //! rate for TIM-TM2 uint8_t tim_rate_; //! raw data stream logging RawDataStreamPa rawDataStreamPa_; - ros::Publisher nav_status_pub_; - ros::Publisher nav_posecef_pub_; - ros::Publisher nav_clock_pub_; - ros::Publisher aid_alm_pub_; - ros::Publisher aid_eph_pub_; - ros::Publisher aid_hui_pub_; + rclcpp::Publisher::SharedPtr nav_status_pub_; + rclcpp::Publisher::SharedPtr nav_posecef_pub_; + rclcpp::Publisher::SharedPtr nav_clock_pub_; + rclcpp::Publisher::SharedPtr aid_alm_pub_; + rclcpp::Publisher::SharedPtr aid_eph_pub_; + rclcpp::Publisher::SharedPtr aid_hui_pub_; //! Navigation rate in measurement cycles, see CfgRate.msg uint16_t nav_rate_; @@ -270,10 +270,7 @@ class UbloxNode final { //! Handles communication with the U-Blox Device std::shared_ptr gps_; - //! Node Handle for GPS node - std::shared_ptr nh_; - - ros::Timer poller_; + rclcpp::TimerBase::SharedPtr poller_; }; } diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 3ae74076..0a4d2185 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -41,10 +41,10 @@ #include // ROS includes -#include +#include // ROS messages -#include +#include /** * @namespace ublox_node @@ -56,7 +56,7 @@ namespace ublox_node { /** * @brief Implements functions for raw data stream. */ -class RawDataStreamPa final { +class RawDataStreamPa final : public rclcpp::Node { public: @@ -93,17 +93,17 @@ class RawDataStreamPa final { void ubloxCallback(const unsigned char* data, const std::size_t size); + private: /** * @brief Callback function which handles raw data. * @param msg ros message */ - void msgCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg); + void msgCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); - private: /** * @brief Converts a string into an uint8 multibyte array */ - std_msgs::UInt8MultiArray str2uint8(const std::string & str); + std_msgs::msg::UInt8MultiArray str2uint8(const std::string & str); /** * @brief Publishes data stream as ros message @@ -132,13 +132,8 @@ class RawDataStreamPa final { //! false: publishing ros messages and/or storing to file bool is_ros_subscriber_; - //! ROS private node handle (for params and publisher) - ros::NodeHandle pnh_; - //! ROS node handle (only for subscriber) - ros::NodeHandle nh_; - - ros::Publisher raw_pub_; - ros::Subscriber raw_data_stream_sub_; + rclcpp::Publisher::SharedPtr raw_pub_; + rclcpp::Subscription::SharedPtr raw_data_stream_sub_; }; } diff --git a/ublox_gps/include/ublox_gps/raw_data_product.hpp b/ublox_gps/include/ublox_gps/raw_data_product.hpp index 57e7d9fd..366bde39 100644 --- a/ublox_gps/include/ublox_gps/raw_data_product.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_product.hpp @@ -4,8 +4,13 @@ #include #include -#include -#include +#include +#include + +#include +#include +#include +#include #include #include @@ -21,7 +26,7 @@ class RawDataProduct final : public virtual ComponentInterface { double kRtcmFreqTol = 0.15; int kRtcmFreqWindow = 25; - explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node); + explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Does nothing since there are no Raw Data product specific settings. @@ -53,15 +58,15 @@ class RawDataProduct final : public virtual ComponentInterface { //! Topic diagnostic updaters std::vector > freq_diagnostics_; - ros::Publisher rxm_raw_pub_; - ros::Publisher rxm_sfrb_pub_; - ros::Publisher rxm_eph_pub_; - ros::Publisher rxm_alm_pub_; + rclcpp::Publisher::SharedPtr rxm_raw_pub_; + rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; + rclcpp::Publisher::SharedPtr rxm_eph_pub_; + rclcpp::Publisher::SharedPtr rxm_alm_pub_; uint16_t nav_rate_; uint16_t meas_rate_; std::shared_ptr updater_; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/tim_product.hpp b/ublox_gps/include/ublox_gps/tim_product.hpp index 5b22defc..57719593 100644 --- a/ublox_gps/include/ublox_gps/tim_product.hpp +++ b/ublox_gps/include/ublox_gps/tim_product.hpp @@ -4,11 +4,13 @@ #include #include -#include -#include -#include +#include +#include +#include -#include +#include +#include +#include #include #include @@ -21,7 +23,7 @@ namespace ublox_node { */ class TimProduct final : public virtual ComponentInterface { public: - explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node); + explicit TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Get the Time Sync parameters. @@ -53,19 +55,19 @@ class TimProduct final : public virtual ComponentInterface { * @brief * @details Publish recieved TimTM2 messages if enabled */ - void callbackTimTM2(const ublox_msgs::TimTM2 &m); + void callbackTimTM2(const ublox_msgs::msg::TimTM2 &m); - sensor_msgs::TimeReference t_ref_; + sensor_msgs::msg::TimeReference t_ref_; - ros::Publisher timtm2_pub_; - ros::Publisher interrupt_time_pub_; - ros::Publisher rxm_sfrb_pub_; - ros::Publisher rxm_raw_pub_; + rclcpp::Publisher::SharedPtr timtm2_pub_; + rclcpp::Publisher::SharedPtr interrupt_time_pub_; + rclcpp::Publisher::SharedPtr rxm_sfrb_pub_; + rclcpp::Publisher::SharedPtr rxm_raw_pub_; std::string frame_id_; std::shared_ptr updater_; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/ublox_firmware.hpp b/ublox_gps/include/ublox_gps/ublox_firmware.hpp index 0afea566..f9f7f804 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware.hpp @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include #include @@ -25,7 +25,7 @@ class UbloxFirmware : public virtual ComponentInterface { //! Subscribe Rate for u-blox SV Info messages constexpr static uint32_t kNavSvInfoSubscribeRate = 20; - explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node); + explicit UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node); /** * @brief Add the fix diagnostics to the updater. @@ -44,7 +44,7 @@ class UbloxFirmware : public virtual ComponentInterface { //! The fix status service type, set in the Firmware Component //! based on the enabled GNSS int fix_status_service_{0}; - ros::NodeHandle* node_; + rclcpp::Node* node_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/ublox_firmware6.hpp b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp index f16b4414..0d7a0af4 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware6.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware6.hpp @@ -4,15 +4,15 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -25,7 +25,7 @@ namespace ublox_node { */ class UbloxFirmware6 final : public UbloxFirmware { public: - explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); + explicit UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); /** * @brief Sets the fix status service type to GPS. @@ -58,7 +58,7 @@ class UbloxFirmware6 final : public UbloxFirmware { * message if publishing is enabled. * @param m the message to process */ - void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m); + void callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m); /** * @brief Update the last known velocity. @@ -66,7 +66,7 @@ class UbloxFirmware6 final : public UbloxFirmware { * @details Publish the message if publishing is enabled. * @param m the message to process */ - void callbackNavVelNed(const ublox_msgs::NavVELNED& m); + void callbackNavVelNed(const ublox_msgs::msg::NavVELNED& m); /** * @brief Update the number of SVs used for the fix. @@ -74,29 +74,29 @@ class UbloxFirmware6 final : public UbloxFirmware { * @details Publish the message if publishing is enabled. * @param m the message to process */ - void callbackNavSol(const ublox_msgs::NavSOL& m); + void callbackNavSol(const ublox_msgs::msg::NavSOL& m); //! The last received navigation position - ublox_msgs::NavPOSLLH last_nav_pos_; + ublox_msgs::msg::NavPOSLLH last_nav_pos_; //! The last received navigation velocity - ublox_msgs::NavVELNED last_nav_vel_; + ublox_msgs::msg::NavVELNED last_nav_vel_; //! The last received num SVs used - ublox_msgs::NavSOL last_nav_sol_; + ublox_msgs::msg::NavSOL last_nav_sol_; //! The last NavSatFix based on last_nav_pos_ - sensor_msgs::NavSatFix fix_; + sensor_msgs::msg::NavSatFix fix_; //! The last Twist based on last_nav_vel_ - geometry_msgs::TwistWithCovarianceStamped velocity_; + geometry_msgs::msg::TwistWithCovarianceStamped velocity_; //! Used to configure NMEA (if set_nmea_) filled with ROS parameters - ublox_msgs::CfgNMEA6 cfg_nmea_; - - ros::Publisher nav_pos_llh_pub_; - ros::Publisher fix_pub_; - ros::Publisher nav_vel_ned_pub_; - ros::Publisher vel_pub_; - ros::Publisher nav_sol_pub_; - ros::Publisher nav_svinfo_pub_; - ros::Publisher mon_hw_pub_; + ublox_msgs::msg::CfgNMEA6 cfg_nmea_; + + rclcpp::Publisher::SharedPtr nav_pos_llh_pub_; + rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr nav_vel_ned_pub_; + rclcpp::Publisher::SharedPtr vel_pub_; + rclcpp::Publisher::SharedPtr nav_sol_pub_; + rclcpp::Publisher::SharedPtr nav_svinfo_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; std::string frame_id_; std::shared_ptr freq_diag_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp index c5e934c2..82b3e1bf 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp @@ -4,13 +4,13 @@ #include #include -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -21,12 +21,12 @@ namespace ublox_node { /** * @brief Implements functions for firmware version 7. */ -class UbloxFirmware7 final : public UbloxFirmware7Plus { +class UbloxFirmware7 final : public UbloxFirmware7Plus { public: - explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_svinfo_pub_ = node->advertise("navsvinfo", 1); - mon_hw_pub_ = node->advertise("monhw", 1); + explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); + mon_hw_pub_ = node->create_publisher("monhw", 1); } /** @@ -53,10 +53,10 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus { /*! * Filled from ROS parameters */ - ublox_msgs::CfgNMEA7 cfg_nmea_; + ublox_msgs::msg::CfgNMEA7 cfg_nmea_; - ros::Publisher nav_svinfo_pub_; - ros::Publisher mon_hw_pub_; + rclcpp::Publisher::SharedPtr nav_svinfo_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp index 6537c1e2..7020a84b 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -4,13 +4,13 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include #include #include @@ -31,15 +31,15 @@ namespace ublox_node { template class UbloxFirmware7Plus : public UbloxFirmware { public: - explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) + explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher - nav_pvt_pub_ = node_->advertise("navpvt", 1); + nav_pvt_pub_ = node_->create_publisher("navpvt", 1); fix_pub_ = - node_->advertise("fix", 1); + node_->create_publisher("fix", 1); vel_pub_ = - node_->advertise("fix_velocity", + node_->create_publisher("fix_velocity", 1); } @@ -52,15 +52,15 @@ class UbloxFirmware7Plus : public UbloxFirmware { * @param m the message to publish */ void callbackNavPvt(const NavPVT& m) { - if (getRosBoolean(node_, "publish/nav/pvt")) { + if (getRosBoolean(node_, "publish.nav.pvt")) { // NavPVT publisher - nav_pvt_pub_.publish(m); + nav_pvt_pub_->publish(m); } // // NavSatFix message // - sensor_msgs::NavSatFix fix; + sensor_msgs::msg::NavSatFix fix; fix.header.frame_id = frame_id_; // set the timestamp uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; @@ -72,15 +72,15 @@ class UbloxFirmware7Plus : public UbloxFirmware { // converted to a positive value if (m.nano < 0) { fix.header.stamp.sec = toUtcSeconds(m) - 1; - fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9); + fix.header.stamp.nanosec = (uint32_t)(m.nano + 1e9); } else { fix.header.stamp.sec = toUtcSeconds(m); - fix.header.stamp.nsec = (uint32_t)(m.nano); + fix.header.stamp.nanosec = (uint32_t)(m.nano); } } else { // Use ROS time since NavPVT timestamp is not valid - fix.header.stamp = ros::Time::now(); + fix.header.stamp = node_->now(); } // Set the LLA fix.latitude = m.lat * 1e-7; // to deg @@ -106,14 +106,14 @@ class UbloxFirmware7Plus : public UbloxFirmware { fix.position_covariance[4] = var_h; fix.position_covariance[8] = var_v; fix.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - fix_pub_.publish(fix); + fix_pub_->publish(fix); // // Twist message // - geometry_msgs::TwistWithCovarianceStamped velocity; + geometry_msgs::msg::TwistWithCovarianceStamped velocity; velocity.header.stamp = fix.header.stamp; velocity.header.frame_id = frame_id_; @@ -129,14 +129,14 @@ class UbloxFirmware7Plus : public UbloxFirmware { velocity.twist.covariance[cols * 2 + 2] = cov_speed; velocity.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - vel_pub_.publish(velocity); + vel_pub_->publish(velocity); // // Update diagnostics // last_nav_pvt_ = m; freq_diag_->diagnostic->tick(fix.header.stamp); - updater_->update(); + updater_->force_update(); } protected: @@ -147,33 +147,33 @@ class UbloxFirmware7Plus : public UbloxFirmware { void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) override { // check the last message, convert to diagnostic if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + ublox_msgs::msg::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_2D) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + } else if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "2D fix"; - } else if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_3D) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + } else if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_3D) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "3D fix"; } else if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + ublox_msgs::msg::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; } else if (last_nav_pvt_.fix_type == - ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + ublox_msgs::msg::NavPVT::FIX_TYPE_TIME_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Time only fix"; } // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + if (!(last_nav_pvt_.flags & ublox_msgs::msg::NavPVT::FLAGS_GNSS_FIX_OK)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_pvt_.fix_type == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + if (last_nav_pvt_.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_NO_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -200,9 +200,9 @@ class UbloxFirmware7Plus : public UbloxFirmware { //! The QZSS Signal configuration, see CfgGNSS message uint32_t qzss_sig_cfg_; - ros::Publisher nav_pvt_pub_; - ros::Publisher fix_pub_; - ros::Publisher vel_pub_; + typename rclcpp::Publisher::SharedPtr nav_pvt_pub_; + rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr vel_pub_; std::string frame_id_; std::shared_ptr freq_diag_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index f0ba0351..0d1158c6 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -4,14 +4,14 @@ #include #include -#include -#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -21,13 +21,13 @@ namespace ublox_node { /** * @brief Implements functions for firmware version 8. */ -class UbloxFirmware8 : public UbloxFirmware7Plus { +class UbloxFirmware8 : public UbloxFirmware7Plus { public: - explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) - : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_sat_pub_ = node->advertise("navstate", 1); - mon_hw_pub_ = node->advertise("monhw", 1); - rxm_rtcm_pub_ = node->advertise("rxmrtcm", 1); + explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) + : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { + nav_sat_pub_ = node->create_publisher("navstate", 1); + mon_hw_pub_ = node->create_publisher("monhw", 1); + rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); } /** @@ -64,14 +64,14 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { //! Whether or not to enable the IMES GNSS bool enable_imes_; //! Desired NMEA configuration. - ublox_msgs::CfgNMEA cfg_nmea_; + ublox_msgs::msg::CfgNMEA cfg_nmea_; //! Whether to clear the flash memory during configuration bool clear_bbr_; bool save_on_shutdown_; - ros::Publisher nav_sat_pub_; - ros::Publisher mon_hw_pub_; - ros::Publisher rxm_rtcm_pub_; + rclcpp::Publisher::SharedPtr nav_sat_pub_; + rclcpp::Publisher::SharedPtr mon_hw_pub_; + rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp index d9b2214b..a9c08476 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -4,8 +4,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -20,7 +20,7 @@ namespace ublox_node { */ class UbloxFirmware9 final : public UbloxFirmware8 { public: - explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node); + explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp index c347f822..3d508c84 100644 --- a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp @@ -4,9 +4,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace ublox_node { diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index b9314d6c..8d78f534 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -9,7 +9,7 @@ #include -#include +#include #include @@ -93,11 +93,12 @@ void checkRange(std::vector val, T min, T max, const std::string & name) { * @return true if found, false if not found. */ template -bool getRosUint(ros::NodeHandle* node, const std::string& key, U &u) { - int param; - if (!node->getParam(key, param)) { +bool getRosUint(rclcpp::Node* node, const std::string& key, U &u) { + rclcpp::Parameter parameter; + if (!node->get_parameter(key, parameter)) { return false; } + int param = parameter.get_value(); // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); @@ -116,7 +117,7 @@ bool getRosUint(ros::NodeHandle* node, const std::string& key, U &u) { * @return true if found, false if not found. */ template -void getRosUint(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { +void getRosUint(rclcpp::Node* node, const std::string& key, U &u, V default_val) { if (!getRosUint(node, key, u)) { u = default_val; } @@ -128,9 +129,9 @@ void getRosUint(ros::NodeHandle* node, const std::string& key, U &u, V default_v * @return true if found, false if not found. */ template -bool getRosUint(ros::NodeHandle* node, const std::string& key, std::vector &u) { - std::vector param; - if (!node->getParam(key, param)) { +bool getRosUint(rclcpp::Node* node, const std::string& key, std::vector &u) { + std::vector param; + if (!node->get_parameter(key, param)) { return false; } @@ -152,11 +153,13 @@ bool getRosUint(ros::NodeHandle* node, const std::string& key, std::vector &u * @return true if found, false if not found. */ template -bool getRosInt(ros::NodeHandle* node, const std::string& key, I &u) { - int param; - if (!node->getParam(key, param)) { +bool getRosInt(rclcpp::Node* node, const std::string& key, I &u) { + rclcpp::Parameter parameter; + if (!node->get_parameter(key, parameter)) { return false; } + + int param = parameter.get_value(); // Check the bounds I min = std::numeric_limits::lowest(); I max = std::numeric_limits::max(); @@ -175,7 +178,7 @@ bool getRosInt(ros::NodeHandle* node, const std::string& key, I &u) { * @return true if found, false if not found. */ template -void getRosInt(ros::NodeHandle* node, const std::string& key, U &u, V default_val) { +void getRosInt(rclcpp::Node* node, const std::string& key, U &u, V default_val) { if (!getRosInt(node, key, u)) { u = default_val; } @@ -187,9 +190,9 @@ void getRosInt(ros::NodeHandle* node, const std::string& key, U &u, V default_va * @return true if found, false if not found. */ template -bool getRosInt(ros::NodeHandle* node, const std::string& key, std::vector &i) { - std::vector param; - if (!node->getParam(key, param)) { +bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { + std::vector param; + if (!node->get_parameter(key, param)) { return false; } @@ -203,29 +206,20 @@ bool getRosInt(ros::NodeHandle* node, const std::string& key, std::vector &i) return true; } -static inline bool declareRosBoolean(ros::NodeHandle* node, const std::string &name, bool default_value) +static inline void declareRosBoolean(rclcpp::Node* node, const std::string &name, bool default_value) { - bool ret; - - if (!node->hasParam(name)) { - node->setParam(name, default_value); - } - // implicit else: If the ROS node already has the parameter, just leave it - - if (!node->getParam(name, ret)) { - throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); - } + node->declare_parameter(name, default_value); } -static inline bool getRosBoolean(ros::NodeHandle* node, const std::string &name) +static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) { - bool ret; - if (!node->getParam(name, ret)) { + rclcpp::Parameter parameter; + if (!node->get_parameter(name, parameter)) { // Note that if this is used after declareRosBoolean, this should never happen. throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); } - return ret; + return parameter.get_value(); } } // namespace ublox_node diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index afde427e..6c2751f2 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -12,13 +12,19 @@ BSD http://ros.org/wiki/ublox - catkin + ament_cmake - ublox_serialization - ublox_msgs - roscpp - roscpp_serialization - tf2 + asio + diagnostic_msgs diagnostic_updater + geometry_msgs + rclcpp + sensor_msgs + std_msgs + ublox_msgs + ublox_serialization + + ament_cmake + diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 77150015..12f4fd40 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -1,20 +1,21 @@ +#include #include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -24,19 +25,19 @@ namespace ublox_node { // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { imu_pub_ = - node_->advertise("imu_meas", 1); + node_->create_publisher("imu_meas", 1); time_ref_pub_ = - node_->advertise("interrupt_time", 1); - nav_att_pub_ = node_->advertise("navatt", 1); - esf_ins_pub_ = node_->advertise("esfins", 1); - esf_meas_pub_ = node_->advertise("esfmeas", 1); - esf_raw_pub_ = node_->advertise("esfraw", 1); - esf_status_pub_ = node_->advertise("esfstatus", 1); - hnr_pvt_pub_ = node_->advertise("hnrpvt", 1); + node_->create_publisher("interrupt_time", 1); + nav_att_pub_ = node_->create_publisher("navatt", 1); + esf_ins_pub_ = node_->create_publisher("esfins", 1); + esf_meas_pub_ = node_->create_publisher("esfmeas", 1); + esf_raw_pub_ = node_->create_publisher("esfraw", 1); + esf_status_pub_ = node_->create_publisher("esfstatus", 1); + hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); } void AdrUdrProduct::getRosParams() { @@ -44,7 +45,7 @@ void AdrUdrProduct::getRosParams() { // Check the nav rate float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { - ROS_WARN("Nav Rate recommended to be 1 Hz"); + RCLCPP_WARN(node_->get_logger(), "Nav Rate recommended to be 1 Hz"); } } @@ -58,56 +59,55 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to NAV ATT messages - if (getRosBoolean(node_, "publish/nav/att")) { - gps->subscribe([this](const ublox_msgs::NavATT &m) { nav_att_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.nav.att")) { + gps->subscribe([this](const ublox_msgs::msg::NavATT &m) { nav_att_pub_->publish(m); }, 1); } // Subscribe to ESF INS messages - if (getRosBoolean(node_, "publish/esf/ins")) { - gps->subscribe([this](const ublox_msgs::EsfINS &m) { esf_ins_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.esf.ins")) { + gps->subscribe([this](const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); }, 1); } // Subscribe to ESF Meas messages - if (getRosBoolean(node_, "publish/esf/meas")) { - gps->subscribe([this](const ublox_msgs::EsfMEAS &m) { esf_meas_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.esf.meas")) { + gps->subscribe([this](const ublox_msgs::msg::EsfMEAS &m) { esf_meas_pub_->publish(m); }, 1); // also publish sensor_msgs::Imu - gps->subscribe(std::bind( + gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); } // Subscribe to ESF Raw messages - if (getRosBoolean(node_, "publish/esf/raw")) { - gps->subscribe([this](const ublox_msgs::EsfRAW &m) { esf_raw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.esf.raw")) { + gps->subscribe([this](const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); }, 1); } // Subscribe to ESF Status messages - if (getRosBoolean(node_, "publish/esf/status")) { - gps->subscribe([this](const ublox_msgs::EsfSTATUS &m) { esf_status_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.esf.status")) { + gps->subscribe([this](const ublox_msgs::msg::EsfSTATUS &m) { esf_status_pub_->publish(m); }, 1); } // Subscribe to HNR PVT messages - if (getRosBoolean(node_, "publish/hnr/pvt")) { - gps->subscribe([this](const ublox_msgs::HnrPVT &m) { hnr_pvt_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.hnr.pvt")) { + gps->subscribe([this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, 1); } } -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { - if (getRosBoolean(node_, "publish/esf/meas")) { - imu_.header.stamp = ros::Time::now(); +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + if (getRosBoolean(node_, "publish.esf.meas")) { + imu_.header.stamp = node_->now(); imu_.header.frame_id = frame_id_; - float deg_per_sec = pow(2, -12); - float m_per_sec_sq = pow(2, -10); - float deg_c = 1e-2; + float deg_per_sec = ::pow(2, -12); + float m_per_sec_sq = ::pow(2, -10); std::vector imu_data = m.data; - for (int i = 0; i < imu_data.size(); i++){ + for (size_t i = 0; i < imu_data.size(); i++){ unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits @@ -118,7 +118,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { data_sign = 1; } - //ROS_INFO("data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray + // RCLCPP_INFO(node_->get_logger(), "data sign (+/-): %f", data_sign); //either 1 or -1....set by bit 23 in the data bitarray imu_.orientation_covariance[0] = -1; imu_.linear_acceleration_covariance[0] = -1; @@ -131,8 +131,8 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { imu_.angular_velocity.x = data_sign * data_value * deg_per_sec; } } else if (data_type == 16) { - //ROS_INFO("data_sign: %f", data_sign); - //ROS_INFO("data_value: %u", data_value * m); + //RCLCPP_INFO(node_->get_logger(), "data_sign: %f", data_sign); + //RCLCPP_INFO(node_->get_logger(), "data_value: %u", data_value * m); if (data_sign == 1) { imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq; } else { @@ -163,28 +163,28 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) { imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq; } } else if (data_type == 12) { - //ROS_INFO("Temperature in celsius: %f", data_value * deg_c); + // RCLCPP_INFO("Temperature in celsius: %f", data_value * deg_c); } else { - ROS_INFO("data_type: %u", data_type); - ROS_INFO("data_value: %u", data_value); + RCLCPP_INFO(node_->get_logger(), "data_type: %u", data_type); + RCLCPP_INFO(node_->get_logger(), "data_value: %u", data_value); } // create time ref message and put in the data //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = ros::Time::now(); + //t_ref_.header.stamp = node_->now(); //t_ref_.header.frame_id = frame_id_; - //t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + //t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); //std::ostringstream src; //src << "TIM" << int(m.ch); //t_ref_.source = src.str(); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.stamp = node_->now(); // create a new timestamp t_ref_.header.frame_id = frame_id_; - time_ref_pub_.publish(t_ref_); - imu_pub_.publish(imu_); + time_ref_pub_->publish(t_ref_); + imu_pub_->publish(imu_); } } diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 768d3d5f..18afd306 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -39,6 +39,7 @@ #include #include +#include namespace ublox_gps { @@ -64,17 +65,17 @@ void Gps::setWorker(const std::shared_ptr& worker) { void Gps::subscribeAcks() { // Set NACK handler - subscribeId(std::bind(&Gps::processNack, this, std::placeholders::_1), + subscribeId(std::bind(&Gps::processNack, this, std::placeholders::_1), ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(std::bind(&Gps::processAck, this, std::placeholders::_1), + subscribeId(std::bind(&Gps::processAck, this, std::placeholders::_1), ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( + subscribe( std::bind(&Gps::processUpdSosAck, this, std::placeholders::_1)); } -void Gps::processAck(const ublox_msgs::Ack &m) { +void Gps::processAck(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages Ack ack; ack.type = ACK; @@ -82,11 +83,11 @@ void Gps::processAck(const ublox_msgs::Ack &m) { ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - ROS_DEBUG_COND(debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - m.cls_id, m.msg_id); + // RCLCPP_DEBUG_COND(debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", + // m.cls_id, m.msg_id); } -void Gps::processNack(const ublox_msgs::Ack &m) { +void Gps::processNack(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages Ack ack; ack.type = NACK; @@ -94,21 +95,21 @@ void Gps::processNack(const ublox_msgs::Ack &m) { ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - ROS_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); + // RCLCPP_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } -void Gps::processUpdSosAck(const ublox_msgs::UpdSOSAck &m) { - if (m.cmd == ublox_msgs::UpdSOSAck::CMD_BACKUP_CREATE_ACK) { +void Gps::processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m) { + if (m.cmd == ublox_msgs::msg::UpdSOSAck::CMD_BACKUP_CREATE_ACK) { Ack ack; ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; ack.class_id = m.CLASS_ID; ack.msg_id = m.MESSAGE_ID; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - ROS_DEBUG_COND(ack.type == ACK && debug_ >= 2, - "U-blox: received UPD SOS Backup ACK"); + // RCLCPP_DEBUG_COND(ack.type == ACK && debug_ >= 2, + // "U-blox: received UPD SOS Backup ACK"); if (ack.type == NACK) { - ROS_ERROR("U-blox: received UPD SOS Backup NACK"); + // RCLCPP_ERROR("U-blox: received UPD SOS Backup NACK"); } } } @@ -127,7 +128,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, + port + " " + e.what()); } - ROS_INFO("U-Blox: Opened serial port %s", port.c_str()); + // RCLCPP_INFO("U-Blox: Opened serial port %s", port.c_str()); int fd = serial->native_handle(); termios tio; @@ -147,7 +148,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (int i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { + for (size_t i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { if (current_baudrate.value() == baudrate) { break; } @@ -160,7 +161,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, std::this_thread::sleep_for( std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); - ROS_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); + // RCLCPP_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } if (config_on_startup_flag_) { configured_ = configUart1(baudrate, uart_in, uart_out); @@ -184,7 +185,7 @@ void Gps::resetSerial(const std::string & port) { + port + " " + e.what()); } - ROS_INFO("U-Blox: Reset serial port %s", port.c_str()); + // RCLCPP_INFO("U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker if (worker_) { @@ -195,15 +196,15 @@ void Gps::resetSerial(const std::string & port) { // Poll UART PRT Config std::vector payload; - payload.push_back(ublox_msgs::CfgPRT::PORT_ID_UART1); - if (!poll(ublox_msgs::CfgPRT::CLASS_ID, ublox_msgs::CfgPRT::MESSAGE_ID, payload)) { - ROS_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); + payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { + // RCLCPP_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } - ublox_msgs::CfgPRT prt; + ublox_msgs::msg::CfgPRT prt; if (!read(prt, default_timeout_)) { - ROS_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - "message"); + // RCLCPP_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", + // "message"); return; } @@ -237,8 +238,8 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { endpoint->service_name() + ": " + e.what()); } - ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - endpoint->service_name().c_str()); + // RCLCPP_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), + // endpoint->service_name().c_str()); if (worker_) { return; @@ -250,9 +251,9 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { void Gps::close() { if (save_on_shutdown_) { if (saveOnShutdown()) { - ROS_INFO("U-Blox Flash BBR saved"); + // RCLCPP_INFO("U-Blox Flash BBR saved"); } else { - ROS_INFO("U-Blox Flash BBR failed to save"); + // RCLCPP_INFO("U-Blox Flash BBR failed to save"); } } worker_.reset(); @@ -272,10 +273,10 @@ void Gps::reset(const std::chrono::milliseconds& wait) { } bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { - ROS_WARN("Resetting u-blox. If device address changes, %s", - "node must be relaunched."); + // RCLCPP_WARN("Resetting u-blox. If device address changes, %s", + // "node must be relaunched."); - ublox_msgs::CfgRST rst; + ublox_msgs::msg::CfgRST rst; rst.nav_bbr_mask = nav_bbr_mask; rst.reset_mode = reset_mode; @@ -286,19 +287,19 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { return true; } -bool Gps::configGnss(ublox_msgs::CfgGNSS gnss, +bool Gps::configGnss(ublox_msgs::msg::CfgGNSS gnss, const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox - ROS_DEBUG("Re-configuring GNSS."); + // RCLCPP_DEBUG("Re-configuring GNSS."); if (!configure(gnss)) { return false; } // Cold reset the GNSS - ROS_WARN("GNSS re-configured, cold resetting device."); - if (!configReset(ublox_msgs::CfgRST::NAV_BBR_COLD_START, ublox_msgs::CfgRST::RESET_MODE_GNSS)) { + // RCLCPP_WARN("GNSS re-configured, cold resetting device."); + if (!configReset(ublox_msgs::msg::CfgRST::NAV_BBR_COLD_START, ublox_msgs::msg::CfgRST::RESET_MODE_GNSS)) { return false; } - ros::Duration(1.0).sleep(); + std::this_thread::sleep_for(std::chrono::seconds(1)); // Reset the I/O reset(wait); return isConfigured(); @@ -306,7 +307,7 @@ bool Gps::configGnss(ublox_msgs::CfgGNSS gnss, bool Gps::saveOnShutdown() { // Command the receiver to stop - ublox_msgs::CfgRST rst; + ublox_msgs::msg::CfgRST rst; rst.nav_bbr_mask = rst.NAV_BBR_HOT_START; rst.reset_mode = rst.RESET_MODE_GNSS_STOP; if (!configure(rst)) { @@ -314,14 +315,14 @@ bool Gps::saveOnShutdown() { } // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - ublox_msgs::UpdSOS backup; + ublox_msgs::msg::UpdSOS backup; return configure(backup); } bool Gps::clearBbr() { // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK - ublox_msgs::UpdSOS sos; + ublox_msgs::msg::UpdSOS sos; sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR; return configure(sos); } @@ -332,36 +333,36 @@ bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, return true; } - ROS_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", - baudrate, in_proto_mask, out_proto_mask); + // RCLCPP_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", + // baudrate, in_proto_mask, out_proto_mask); - ublox_msgs::CfgPRT port; - port.port_id = ublox_msgs::CfgPRT::PORT_ID_UART1; + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_UART1; port.baud_rate = baudrate; - port.mode = ublox_msgs::CfgPRT::MODE_RESERVED1 | ublox_msgs::CfgPRT::MODE_CHAR_LEN_8BIT | - ublox_msgs::CfgPRT::MODE_PARITY_NO | ublox_msgs::CfgPRT::MODE_STOP_BITS_1; + port.mode = ublox_msgs::msg::CfgPRT::MODE_RESERVED1 | ublox_msgs::msg::CfgPRT::MODE_CHAR_LEN_8BIT | + ublox_msgs::msg::CfgPRT::MODE_PARITY_NO | ublox_msgs::msg::CfgPRT::MODE_STOP_BITS_1; port.in_proto_mask = in_proto_mask; port.out_proto_mask = out_proto_mask; return configure(port); } -bool Gps::disableUart1(ublox_msgs::CfgPRT& prev_config) { - ROS_DEBUG("Disabling UART1"); +bool Gps::disableUart1(ublox_msgs::msg::CfgPRT& prev_config) { + // RCLCPP_DEBUG("Disabling UART1"); // Poll UART PRT Config std::vector payload; - payload.push_back(ublox_msgs::CfgPRT::PORT_ID_UART1); - if (!poll(ublox_msgs::CfgPRT::CLASS_ID, ublox_msgs::CfgPRT::MESSAGE_ID, payload)) { - ROS_ERROR("disableUart: Could not poll UART1 CfgPRT"); + payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); + if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { + // RCLCPP_ERROR("disableUart: Could not poll UART1 CfgPRT"); return false; } if (!read(prev_config, default_timeout_)) { - ROS_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); + // RCLCPP_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); return false; } // Keep original settings, but disable in/out - ublox_msgs::CfgPRT port; - port.port_id = ublox_msgs::CfgPRT::PORT_ID_UART1; + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_UART1; port.mode = prev_config.mode; port.baud_rate = prev_config.baud_rate; port.in_proto_mask = 0; @@ -378,11 +379,11 @@ bool Gps::configUsb(uint16_t tx_ready, return true; } - ROS_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - tx_ready, in_proto_mask, out_proto_mask); + // RCLCPP_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", + // tx_ready, in_proto_mask, out_proto_mask); - ublox_msgs::CfgPRT port; - port.port_id = ublox_msgs::CfgPRT::PORT_ID_USB; + ublox_msgs::msg::CfgPRT port; + port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_USB; port.tx_ready = tx_ready; port.in_proto_mask = in_proto_mask; port.out_proto_mask = out_proto_mask; @@ -390,21 +391,21 @@ bool Gps::configUsb(uint16_t tx_ready, } bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { - ROS_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", - meas_rate, nav_rate); + // RCLCPP_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", + // meas_rate, nav_rate); - ublox_msgs::CfgRATE rate; + ublox_msgs::msg::CfgRATE rate; rate.meas_rate = meas_rate; rate.nav_rate = nav_rate; // must be fixed at 1 for ublox 5 and 6 - rate.time_ref = ublox_msgs::CfgRATE::TIME_REF_GPS; + rate.time_ref = ublox_msgs::msg::CfgRATE::TIME_REF_GPS; return configure(rate); } bool Gps::configRtcm(const std::vector & rtcms) { for (size_t i = 0; i < rtcms.size(); ++i) { - ROS_DEBUG("Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); + // RCLCPP_DEBUG("Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); if (!setRate(ublox_msgs::Class::RTCM, rtcms[i].id, rtcms[i].rate)) { - ROS_ERROR("Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); + // RCLCPP_ERROR("Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); return false; } } @@ -412,28 +413,28 @@ bool Gps::configRtcm(const std::vector & rtcms) { } bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { - ROS_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); + // RCLCPP_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); - ublox_msgs::CfgSBAS msg; - msg.mode = (enable ? ublox_msgs::CfgSBAS::MODE_ENABLED : 0); + ublox_msgs::msg::CfgSBAS msg; + msg.mode = (enable ? ublox_msgs::msg::CfgSBAS::MODE_ENABLED : 0); msg.usage = usage; msg.max_sbas = max_sbas; return configure(msg); } bool Gps::configTmode3Fixed(bool lla_flag, - std::vector arp_position, + std::vector arp_position, std::vector arp_position_hp, float fixed_pos_acc) { if (arp_position.size() != 3 || arp_position_hp.size() != 3) { - ROS_ERROR("Configuring TMODE3 to Fixed: size of position %s", - "& arp_position_hp args must be 3"); + // RCLCPP_ERROR("Configuring TMODE3 to Fixed: size of position %s", + // "& arp_position_hp args must be 3"); return false; } - ROS_DEBUG("Configuring TMODE3 to Fixed"); + // RCLCPP_DEBUG("Configuring TMODE3 to Fixed"); - ublox_msgs::CfgTMODE3 tmode3; + ublox_msgs::msg::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK; tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; @@ -459,8 +460,8 @@ bool Gps::configTmode3Fixed(bool lla_flag, bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) { - ublox_msgs::CfgTMODE3 tmode3; - ROS_DEBUG("Setting TMODE3 to Survey In"); + ublox_msgs::msg::CfgTMODE3 tmode3; + // RCLCPP_DEBUG("Setting TMODE3 to Survey In"); tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; tmode3.svin_min_dur = svin_min_dur; // Convert from m to [0.1 mm] @@ -469,17 +470,17 @@ bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, } bool Gps::disableTmode3() { - ROS_DEBUG("Disabling TMODE3"); + // RCLCPP_DEBUG("Disabling TMODE3"); - ublox_msgs::CfgTMODE3 tmode3; + ublox_msgs::msg::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK; return configure(tmode3); } bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - ROS_DEBUG_COND(debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, - message_id, rate); - ublox_msgs::CfgMSG msg; + // RCLCPP_DEBUG_COND(debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, + // message_id, rate); + ublox_msgs::msg::CfgMSG msg; msg.msg_class = class_id; msg.msg_id = message_id; msg.rate = rate; @@ -487,54 +488,54 @@ bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { } bool Gps::setDynamicModel(uint8_t model) { - ROS_DEBUG("Setting dynamic model to %u", model); + // RCLCPP_DEBUG("Setting dynamic model to %u", model); - ublox_msgs::CfgNAV5 msg; + ublox_msgs::msg::CfgNAV5 msg; msg.dyn_model = model; - msg.mask = ublox_msgs::CfgNAV5::MASK_DYN; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_DYN; return configure(msg); } bool Gps::setFixMode(uint8_t mode) { - ROS_DEBUG("Setting fix mode to %u", mode); + // RCLCPP_DEBUG("Setting fix mode to %u", mode); - ublox_msgs::CfgNAV5 msg; + ublox_msgs::msg::CfgNAV5 msg; msg.fix_mode = mode; - msg.mask = ublox_msgs::CfgNAV5::MASK_FIX_MODE; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_FIX_MODE; return configure(msg); } bool Gps::setDeadReckonLimit(uint8_t limit) { - ROS_DEBUG("Setting DR Limit to %u", limit); + // RCLCPP_DEBUG("Setting DR Limit to %u", limit); - ublox_msgs::CfgNAV5 msg; + ublox_msgs::msg::CfgNAV5 msg; msg.dr_limit = limit; - msg.mask = ublox_msgs::CfgNAV5::MASK_DR_LIM; + msg.mask = ublox_msgs::msg::CfgNAV5::MASK_DR_LIM; return configure(msg); } bool Gps::setPpp(bool enable) { - ROS_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); + // RCLCPP_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); - ublox_msgs::CfgNAVX5 msg; + ublox_msgs::msg::CfgNAVX5 msg; msg.use_ppp = enable; - msg.mask1 = ublox_msgs::CfgNAVX5::MASK1_PPP; + msg.mask1 = ublox_msgs::msg::CfgNAVX5::MASK1_PPP; return configure(msg); } bool Gps::setDgnss(uint8_t mode) { - ublox_msgs::CfgDGNSS cfg; - ROS_DEBUG("Setting DGNSS mode to %u", mode); + ublox_msgs::msg::CfgDGNSS cfg; + // RCLCPP_DEBUG("Setting DGNSS mode to %u", mode); cfg.dgnss_mode = mode; return configure(cfg); } bool Gps::setUseAdr(bool enable) { - ROS_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); + // RCLCPP_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); - ublox_msgs::CfgNAVX5 msg; + ublox_msgs::msg::CfgNAVX5 msg; msg.use_adr = enable; - msg.mask2 = ublox_msgs::CfgNAVX5::MASK2_ADR; + msg.mask2 = ublox_msgs::msg::CfgNAVX5::MASK2_ADR; return configure(msg); } @@ -556,8 +557,8 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id) { - ROS_DEBUG_COND(debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", - class_id, msg_id); + // RCLCPP_DEBUG_COND(debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", + // class_id, msg_id); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::system_clock::time_point wait_until = now + timeout; @@ -583,19 +584,20 @@ void Gps::setRawDataCallback(const Worker::Callback& callback) { } bool Gps::setUTCtime() { - ROS_DEBUG("Setting time to UTC time"); + // RCLCPP_DEBUG("Setting time to UTC time"); - ublox_msgs::CfgNAV5 msg; + ublox_msgs::msg::CfgNAV5 msg; msg.utc_standard = 3; return configure(msg); } bool Gps::setTimtm2(uint8_t rate) { - ROS_DEBUG("TIM-TM2 send rate on current port set to %u", rate); - ublox_msgs::CfgMSG msg; - msg.msg_class = ublox_msgs::TimTM2::CLASS_ID; - msg.msg_id = ublox_msgs::TimTM2::MESSAGE_ID; + // RCLCPP_DEBUG("TIM-TM2 send rate on current port set to %u", rate); + ublox_msgs::msg::CfgMSG msg; + msg.msg_class = ublox_msgs::msg::TimTM2::CLASS_ID; + msg.msg_id = ublox_msgs::msg::TimTM2::MESSAGE_ID; msg.rate = rate; return configure(msg); } + } // namespace ublox_gps diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index 40f956a5..4d3ae135 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -1,14 +1,15 @@ +#include #include #include #include #include -#include -#include -#include +#include +#include +#include #include -#include +#include #include #include @@ -19,30 +20,30 @@ namespace ublox_node { // // U-Blox High Precision Positioning Receiver // -HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) +HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) { nav_relposned_pub_ = - node_->advertise("navrelposned", 1); + node_->create_publisher("navrelposned", 1); imu_pub_ = - node_->advertise("navheading", 1); + node_->create_publisher("navheading", 1); } void HpPosRecProduct::subscribe(std::shared_ptr gps) { // Whether to publish Nav Relative Position NED // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps->subscribe(std::bind( + gps->subscribe(std::bind( &HpPosRecProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); } -void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { - if (getRosBoolean(node_, "publish/nav/relposned")) { - nav_relposned_pub_.publish(m); +void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 &m) { + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_relposned_pub_->publish(m); } - if (getRosBoolean(node_, "publish/nav/heading")) { - imu_.header.stamp = ros::Time::now(); + if (getRosBoolean(node_, "publish.nav.heading")) { + imu_.header.stamp = node_->now(); imu_.header.frame_id = frame_id_; imu_.linear_acceleration_covariance[0] = -1; @@ -58,13 +59,13 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) { // Only heading is reported with an accuracy in 0.1mm units imu_.orientation_covariance[0] = 1000.0; imu_.orientation_covariance[4] = 1000.0; - imu_.orientation_covariance[8] = pow(m.acc_heading / 10000.0, 2); + imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2); - imu_pub_.publish(imu_); + imu_pub_->publish(imu_); } last_rel_pos_ = m; - updater_->update(); + updater_->force_update(); } } // namespace ublox_node diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index a7bd8567..a721b0a4 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -4,12 +4,12 @@ #include #include -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include #include @@ -21,52 +21,52 @@ namespace ublox_node { // u-blox High Precision GNSS Reference Station // -HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, ros::NodeHandle* node) +HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) { navsvin_pub_ = - node_->advertise("navsvin", 1); + node_->create_publisher("navsvin", 1); } void HpgRefProduct::getRosParams() { if (getRosBoolean(node_, "config_on_startup")) { if (nav_rate_ * meas_rate_ != 1000) { - ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz."); + RCLCPP_WARN(node_->get_logger(), "For HPG Ref devices, nav_rate should be exactly 1 Hz."); } if (!getRosUint(node_, "tmode3", tmode3_)) { throw std::runtime_error("Invalid settings: TMODE3 must be set"); } - if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { - if (!node_->getParam("arp/position", arp_position_)) { + if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED) { + if (!node_->get_parameter("arp.position", arp_position_)) { throw std::runtime_error(std::string("Invalid settings: arp/position ") + "must be set if TMODE3 is fixed"); } - if (!getRosInt(node_, "arp/position_hp", arp_position_hp_)) { + if (!getRosInt(node_, "arp.position_hp", arp_position_hp_)) { throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + "must be set if TMODE3 is fixed"); } - if (!node_->getParam("arp/acc", fixed_pos_acc_)) { + if (!node_->get_parameter("arp.acc", fixed_pos_acc_)) { throw std::runtime_error(std::string("Invalid settings: arp/acc ") + "must be set if TMODE3 is fixed"); } - if (!node_->getParam("arp/lla_flag", lla_flag_)) { - ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s", + if (!node_->get_parameter("arp.lla_flag", lla_flag_)) { + RCLCPP_WARN(node_->get_logger(), "arp/lla_flag param not set, assuming ARP coordinates are %s", "in ECEF"); lla_flag_ = false; } - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { - svin_reset_ = getRosBoolean(node_, "sv_in/reset"); - if (!getRosUint(node_, "sv_in/min_dur", sv_in_min_dur_)) { + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + svin_reset_ = getRosBoolean(node_, "sv_in.reset"); + if (!getRosUint(node_, "sv_in.min_dur", sv_in_min_dur_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ") + "must be set if TMODE3 is survey-in"); } - if (!node_->getParam("sv_in/acc_lim", sv_in_acc_lim_)) { + if (!node_->get_parameter("sv_in.acc_lim", sv_in_acc_lim_)) { throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ") + "must be set if TMODE3 is survey-in"); } - } else if (tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + } else if (tmode3_ != ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED) { throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3") + " flag constants for possible values."); } @@ -75,12 +75,12 @@ void HpgRefProduct::getRosParams() { bool HpgRefProduct::configureUblox(std::shared_ptr gps) { // Configure TMODE3 - if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) { + if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED) { if (!gps->disableTmode3()) { throw std::runtime_error("Failed to disable TMODE3."); } mode_ = DISABLED; - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) { + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED) { if (!gps->configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_, fixed_pos_acc_)) { throw std::runtime_error("Failed to set TMODE3 to fixed."); @@ -89,9 +89,9 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { throw std::runtime_error("Failed to set RTCM rates"); } mode_ = FIXED; - } else if (tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { + } else if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN) { if (!svin_reset_) { - ublox_msgs::NavSVIN nav_svin; + ublox_msgs::msg::NavSVIN nav_svin; if (!gps->poll(nav_svin)) { throw std::runtime_error(std::string("Failed to poll NavSVIN while") + " configuring survey-in"); @@ -106,7 +106,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { setTimeMode(gps); return true; } - ublox_msgs::NavPVT nav_pvt; + ublox_msgs::msg::NavPVT nav_pvt; if (!gps->poll(nav_pvt)) { throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); @@ -132,7 +132,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { } // As recommended in the documentation, first disable, then set to survey in if (!gps->disableTmode3()) { - ROS_ERROR("Failed to disable TMODE3 before setting to survey-in."); + RCLCPP_ERROR(node_->get_logger(), "Failed to disable TMODE3 before setting to survey-in."); } else { mode_ = DISABLED; } @@ -149,13 +149,13 @@ void HpgRefProduct::subscribe(std::shared_ptr gps) { // Subscribe to Nav Survey-In // Save off the gps pointer so we can use it in the callback later. gps_ = gps; - gps->subscribe(std::bind( + gps->subscribe(std::bind( &HpgRefProduct::callbackNavSvIn, this, std::placeholders::_1), 1); } -void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { - if (getRosBoolean(node_, "publish/nav/svin")) { - navsvin_pub_.publish(m); +void HpgRefProduct::callbackNavSvIn(const ublox_msgs::msg::NavSVIN& m) { + if (getRosBoolean(node_, "publish.nav.svin")) { + navsvin_pub_->publish(m); } last_nav_svin_ = m; @@ -164,22 +164,22 @@ void HpgRefProduct::callbackNavSvIn(const ublox_msgs::NavSVIN& m) { setTimeMode(gps_); } - updater_->update(); + updater_->force_update(); } bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { - ROS_INFO("Setting mode (internal state) to Time Mode"); + RCLCPP_INFO(node_->get_logger(), "Setting mode (internal state) to Time Mode"); mode_ = TIME; // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) if (!gps->configRate(meas_rate_, nav_rate_)) { - ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate_, + RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms %s %d", meas_rate_, "navigation rate to ", nav_rate_); } // Enable the RTCM out messages if (!gps->configRtcm(rtcms_)) { - ROS_ERROR("Failed to configure RTCM IDs"); + RCLCPP_ERROR(node_->get_logger(), "Failed to configure RTCM IDs"); return false; } return true; @@ -193,23 +193,23 @@ void HpgRefProduct::initializeRosDiagnostics() { void HpgRefProduct::tmode3Diagnostics( diagnostic_updater::DiagnosticStatusWrapper& stat) { if (mode_ == INIT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Not configured"; } else if (mode_ == DISABLED){ - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Disabled"; } else if (mode_ == SURVEY_IN) { if (!last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat.message = "Survey-In inactive and invalid"; } else if (last_nav_svin_.active && !last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Survey-In active but invalid"; } else if (!last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Survey-In complete"; } else if (last_nav_svin_.active && last_nav_svin_.valid) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Survey-In active and valid"; } @@ -224,10 +224,10 @@ void HpgRefProduct::tmode3Diagnostics( stat.add("Mean Z HP [m]", last_nav_svin_.mean_zhp * 1e-4); stat.add("Mean Accuracy [m]", last_nav_svin_.mean_acc * 1e-4); } else if (mode_ == FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Fixed Position"; } else if (mode_ == TIME) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Time"; } } diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp index 7c5042c6..865c6f39 100644 --- a/ublox_gps/src/hpg_rov_product.cpp +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -3,12 +3,12 @@ #include #include -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include #include @@ -20,17 +20,17 @@ namespace ublox_node { // // U-Blox High Precision GNSS Rover // -HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, ros::NodeHandle* node) +HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node) : nav_rate_(nav_rate), updater_(updater), node_(node) { nav_rel_pos_ned_pub_ = - node_->advertise("navrelposned", 1); + node_->create_publisher("navrelposned", 1); } void HpgRovProduct::getRosParams() { // default to float, see CfgDGNSS message for details getRosUint(node_, "dgnss_mode", dgnss_mode_, - ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED); + ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } bool HpgRovProduct::configureUblox(std::shared_ptr gps) { @@ -43,7 +43,7 @@ bool HpgRovProduct::configureUblox(std::shared_ptr gps) { void HpgRovProduct::subscribe(std::shared_ptr gps) { // Subscribe to Nav Relative Position NED messages (also updates diagnostics) - gps->subscribe(std::bind( + gps->subscribe(std::bind( &HpgRovProduct::callbackNavRelPosNed, this, std::placeholders::_1), 1); } @@ -63,14 +63,14 @@ void HpgRovProduct::carrierPhaseDiagnostics( if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat.message = "None"; } else { if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Float"; } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Fixed"; } stat.add("Ref Station ID", last_rel_pos_.ref_station_id); @@ -90,13 +90,13 @@ void HpgRovProduct::carrierPhaseDiagnostics( } } -void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) { - if (getRosBoolean(node_, "publish/nav/relposned")) { - nav_rel_pos_ned_pub_.publish(m); +void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m) { + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_rel_pos_ned_pub_->publish(m); } last_rel_pos_ = m; - updater_->update(); + updater_->force_update(); } } // namespace ublox_node diff --git a/ublox_gps/src/logger_node_pa.cpp b/ublox_gps/src/logger_node_pa.cpp index d18c2031..4ec94fb5 100644 --- a/ublox_gps/src/logger_node_pa.cpp +++ b/ublox_gps/src/logger_node_pa.cpp @@ -33,7 +33,7 @@ // measured data with the rtklib. // ROS includes -#include +#include // Ublox GPS includes #include @@ -43,13 +43,15 @@ int main(int argc, char** argv) { - ros::init(argc, argv, "ublox_logger"); + rclcpp::init(argc, argv); - ublox_node::RawDataStreamPa node(true); - node.getRosParams(); - node.initialize(); + auto node = std::make_shared(true); + node->getRosParams(); + node->initialize(); - ros::spin(); + rclcpp::spin(node); - return 0; + rclcpp::shutdown(); + + return 0; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f49e4eea..fe910c6a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -38,18 +38,18 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -87,23 +87,23 @@ uint8_t modelFromString(const std::string& model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "portable") { - return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PORTABLE; } else if (lower == "stationary") { - return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_STATIONARY; } else if (lower == "pedestrian") { - return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PEDESTRIAN; } else if (lower == "automotive") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AUTOMOTIVE; } else if (lower == "sea") { - return ublox_msgs::CfgNAV5::DYN_MODEL_SEA; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_SEA; } else if (lower == "airborne1") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_1G; } else if (lower == "airborne2") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_2G; } else if (lower == "airborne4") { - return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_4G; } else if (lower == "wristwatch") { - return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH; + return ublox_msgs::msg::CfgNAV5::DYN_MODEL_WRIST_WATCH; } throw std::runtime_error("Invalid settings: " + lower + @@ -123,11 +123,11 @@ uint8_t fixModeFromString(const std::string& mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { - return ublox_msgs::CfgNAV5::FIX_MODE_2D_ONLY; + return ublox_msgs::msg::CfgNAV5::FIX_MODE_2D_ONLY; } else if (lower == "3d") { - return ublox_msgs::CfgNAV5::FIX_MODE_3D_ONLY; + return ublox_msgs::msg::CfgNAV5::FIX_MODE_3D_ONLY; } else if (lower == "auto") { - return ublox_msgs::CfgNAV5::FIX_MODE_AUTO; + return ublox_msgs::msg::CfgNAV5::FIX_MODE_AUTO; } throw std::runtime_error("Invalid settings: " + mode + @@ -158,30 +158,27 @@ std::vector stringSplit(const std::string &str, // // u-blox ROS Node // -UbloxNode::UbloxNode() { - nh_ = std::make_shared("~"); - - int debug; - nh_->param("debug", debug, 1); +UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { + int debug = this->declare_parameter("debug", 1); if (debug) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } + // if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + // ros::console::levels::Debug)) { + // ros::console::notifyLoggerLevelsChanged(); + // } } gps_ = std::make_shared(debug); gnss_ = std::make_shared(); - nav_status_pub_ = nh_->advertise("navstatus", 1); - nav_posecef_pub_ = nh_->advertise("navposecef", 1); - nav_clock_pub_ = nh_->advertise("navclock", 1); - aid_alm_pub_ = nh_->advertise("aidalm", 1); - aid_eph_pub_ = nh_->advertise("aideph", 1); - aid_hui_pub_ = nh_->advertise("aidhui", 1); + nav_status_pub_ = this->create_publisher("navstatus", 1); + nav_posecef_pub_ = this->create_publisher("navposecef", 1); + nav_clock_pub_ = this->create_publisher("navclock", 1); + aid_alm_pub_ = this->create_publisher("aidalm", 1); + aid_eph_pub_ = this->create_publisher("aideph", 1); + aid_hui_pub_ = this->create_publisher("aidhui", 1); - updater_ = std::make_shared(); + updater_ = std::make_shared(this); updater_->setHardwareID("ublox"); // configure diagnostic updater for frequency @@ -194,101 +191,101 @@ UbloxNode::UbloxNode() { void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 6; } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 7; } else if (protocol_version_ > 15 && protocol_version_ <= 23) { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 8; } else { - components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, nh_.get())); + components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 9; } - ROS_INFO("U-Blox Firmware Version: %d", ublox_version); + RCLCPP_INFO(this->get_logger(), "U-Blox Firmware Version: %d", ublox_version); } void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, nh_.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { - components_.push_back(std::make_shared(nav_rate_, updater_, nh_.get())); + components_.push_back(std::make_shared(nav_rate_, updater_, this)); } else if (product_category.compare("HPG") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, nh_.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); } else if (product_category.compare("TIM") == 0) { - components_.push_back(std::make_shared(frame_id_, updater_, nh_.get())); + components_.push_back(std::make_shared(frame_id_, updater_, this)); } else if (product_category.compare("ADR") == 0 || product_category.compare("UDR") == 0) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, nh_.get())); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); } else if (product_category.compare("FTS") == 0) { components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { - ROS_WARN("Product category %s %s from MonVER message not recognized %s", + RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); } } void UbloxNode::getRosParams() { - nh_->param("device", device_, std::string("/dev/ttyACM0")); - nh_->param("frame_id", frame_id_, std::string("gps")); + device_ = this->declare_parameter("device", std::string("/dev/ttyACM0")); + frame_id_ = this->declare_parameter("frame_id", std::string("gps")); // Save configuration parameters - getRosUint(nh_.get(), "load/mask", load_.load_mask, 0); - getRosUint(nh_.get(), "load/device", load_.device_mask, 0); - getRosUint(nh_.get(), "save/mask", save_.save_mask, 0); - getRosUint(nh_.get(), "save/device", save_.device_mask, 0); + getRosUint(this, "load.mask", load_.load_mask, 0); + getRosUint(this, "load.device", load_.device_mask, 0); + getRosUint(this, "save.mask", save_.save_mask, 0); + getRosUint(this, "save.device", save_.device_mask, 0); // UART 1 params - getRosUint(nh_.get(), "uart1/baudrate", baudrate_, 9600); - getRosUint(nh_.get(), "uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX - | ublox_msgs::CfgPRT::PROTO_NMEA - | ublox_msgs::CfgPRT::PROTO_RTCM); - getRosUint(nh_.get(), "uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX); + getRosUint(this, "uart1.baudrate", baudrate_, 9600); + getRosUint(this, "uart1.in", uart_in_, ublox_msgs::msg::CfgPRT::PROTO_UBX + | ublox_msgs::msg::CfgPRT::PROTO_NMEA + | ublox_msgs::msg::CfgPRT::PROTO_RTCM); + getRosUint(this, "uart1.out", uart_out_, ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (nh_->hasParam("usb/in") || nh_->hasParam("usb/out")) { + if (this->has_parameter("usb.in") || this->has_parameter("usb.out")) { set_usb_ = true; - if (!getRosUint(nh_.get(), "usb/in", usb_in_)) { - throw std::runtime_error(std::string("usb/out is set, therefore ") + - "usb/in must be set"); + if (!getRosUint(this, "usb.in", usb_in_)) { + throw std::runtime_error(std::string("usb.out is set, therefore ") + + "usb.in must be set"); } - if (!getRosUint(nh_.get(), "usb/out", usb_out_)) { - throw std::runtime_error(std::string("usb/in is set, therefore ") + - "usb/out must be set"); + if (!getRosUint(this, "usb.out", usb_out_)) { + throw std::runtime_error(std::string("usb.in is set, therefore ") + + "usb.out must be set"); } - getRosUint(nh_.get(), "usb/tx_ready", usb_tx_, 0); + getRosUint(this, "usb.tx_ready", usb_tx_, 0); } // Measurement rate params - nh_->param("rate", rate_, 4.0); // in Hz - getRosUint(nh_.get(), "nav_rate", nav_rate_, 1); // # of measurement rate cycles + rate_ = this->declare_parameter("rate", 4.0); // in Hz + getRosUint(this, "nav_rate", nav_rate_, 1); // # of measurement rate cycles // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; - getRosUint(nh_.get(), "rtcm/ids", rtcm_ids); // RTCM output message IDs - getRosUint(nh_.get(), "rtcm/rates", rtcm_rates); // RTCM output message rates + getRosUint(this, "rtcm.ids", rtcm_ids); // RTCM output message IDs + getRosUint(this, "rtcm.rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting - declareRosBoolean(nh_.get(), "enable_ppp", false); + declareRosBoolean(this, "enable_ppp", false); // SBAS params, only for some devices - declareRosBoolean(nh_.get(), "gnss/sbas", false); - declareRosBoolean(nh_.get(), "gnss/gps", true); - declareRosBoolean(nh_.get(), "gnss/glonass", false); - declareRosBoolean(nh_.get(), "gnss/qzss", false); - declareRosBoolean(nh_.get(), "gnss/galileo", false); - declareRosBoolean(nh_.get(), "gnss/beidou", false); - declareRosBoolean(nh_.get(), "gnss/imes", false); - getRosUint(nh_.get(), "sbas/max", max_sbas_, 0); // Maximum number of SBAS channels - getRosUint(nh_.get(), "sbas/usage", sbas_usage_, 0); - nh_->param("dynamic_model", dynamic_model_, std::string("portable")); - nh_->param("fix_mode", fix_mode_, std::string("auto")); - getRosUint(nh_.get(), "dr_limit", dr_limit_, 0); // Dead reckoning limit - - if (getRosBoolean(nh_.get(), "enable_ppp")) { - ROS_WARN("Warning: PPP is enabled - this is an expert setting."); + declareRosBoolean(this, "gnss.sbas", false); + declareRosBoolean(this, "gnss.gps", true); + declareRosBoolean(this, "gnss.glonass", false); + declareRosBoolean(this, "gnss.qzss", false); + declareRosBoolean(this, "gnss.galileo", false); + declareRosBoolean(this, "gnss.beidou", false); + declareRosBoolean(this, "gnss.imes", false); + getRosUint(this, "sbas.max", max_sbas_, 0); // Maximum number of SBAS channels + getRosUint(this, "sbas.usage", sbas_usage_, 0); + dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); + fix_mode_ = this->declare_parameter("fix_mode", std::string("auto")); + getRosUint(this, "dr_limit", dr_limit_, 0); // Dead reckoning limit + + if (getRosBoolean(this, "enable_ppp")) { + RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); } checkMin(rate_, 0, "rate"); @@ -307,30 +304,30 @@ void UbloxNode::getRosParams() { dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); - declareRosBoolean(nh_.get(), "dat/set", false); - if (getRosBoolean(nh_.get(), "dat/set")) { - std::vector shift, rot; - if (!nh_->getParam("dat/majA", cfg_dat_.maj_a) - || nh_->getParam("dat/flat", cfg_dat_.flat) - || nh_->getParam("dat/shift", shift) - || nh_->getParam("dat/rot", rot) - || nh_->getParam("dat/scale", cfg_dat_.scale)) { - throw std::runtime_error(std::string("dat/set is true, therefore ") + - "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set"); + declareRosBoolean(this, "dat.set", false); + if (getRosBoolean(this, "dat.set")) { + std::vector shift, rot; + if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) + || this->get_parameter("dat.flat", cfg_dat_.flat) + || this->get_parameter("dat.shift", shift) + || this->get_parameter("dat.rot", rot) + || this->get_parameter("dat.scale", cfg_dat_.scale)) { + throw std::runtime_error(std::string("dat.set is true, therefore ") + + "dat.majA, dat.flat, dat.shift, dat.rot, & dat.scale must be set"); } if (shift.size() != 3 || rot.size() != 3) { - throw std::runtime_error(std::string("size of dat/shift & dat/rot ") + + throw std::runtime_error(std::string("size of dat.shift & dat.rot ") + "must be 3"); } - checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat/majA"); - checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat"); + checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat.majA"); + checkRange(cfg_dat_.flat, 0.0, 500.0, "dat.flat"); - checkRange(shift, 0.0, 500.0, "dat/shift"); + checkRange(shift, 0.0, 500.0, "dat.shift"); cfg_dat_.d_x = shift[0]; cfg_dat_.d_y = shift[1]; cfg_dat_.d_z = shift[2]; - checkRange(rot, -5000.0, 5000.0, "dat/rot"); + checkRange(rot, -5000.0, 5000.0, "dat.rot"); cfg_dat_.rot_x = rot[0]; cfg_dat_.rot_y = rot[1]; cfg_dat_.rot_z = rot[2]; @@ -342,99 +339,111 @@ void UbloxNode::getRosParams() { meas_rate_ = 1000 / rate_; // activate/deactivate any config - declareRosBoolean(nh_.get(), "config_on_startup", true); - declareRosBoolean(nh_.get(), "raw_data", false); - declareRosBoolean(nh_.get(), "clear_bbr", false); - declareRosBoolean(nh_.get(), "save_on_shutdown", false); - declareRosBoolean(nh_.get(), "use_adr", true); + declareRosBoolean(this, "config_on_startup", true); + declareRosBoolean(this, "raw_data", false); + declareRosBoolean(this, "clear_bbr", false); + declareRosBoolean(this, "save_on_shutdown", false); + declareRosBoolean(this, "use_adr", true); - declareRosBoolean(nh_.get(), "sv_in/reset", true); + declareRosBoolean(this, "sv_in.reset", true); + this->declare_parameter("sv_in.min_dur", 0); + this->declare_parameter("sv_in.acc_lim", 0.0); + + this->declare_parameter("dgnss_mode", ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FLOAT); // raw data stream logging rawDataStreamPa_.getRosParams(); // NMEA parameters - declareRosBoolean(nh_.get(), "nmea/set", false); - declareRosBoolean(nh_.get(), "nmea/compat", false); - declareRosBoolean(nh_.get(), "nmea/consider", false); - declareRosBoolean(nh_.get(), "nmea/limit82", false); - declareRosBoolean(nh_.get(), "nmea/high_prec", false); - declareRosBoolean(nh_.get(), "nmea/filter/pos", false); - declareRosBoolean(nh_.get(), "nmea/filter/msk_pos", false); - declareRosBoolean(nh_.get(), "nmea/filter/time", false); - declareRosBoolean(nh_.get(), "nmea/filter/date", false); - declareRosBoolean(nh_.get(), "nmea/filter/sbas", false); - declareRosBoolean(nh_.get(), "nmea/filter/track", false); - declareRosBoolean(nh_.get(), "nmea/filter/gps_only", false); - declareRosBoolean(nh_.get(), "nmea/gnssToFilter/gps", false); - declareRosBoolean(nh_.get(), "nmea/gnssToFilter/sbas", false); - declareRosBoolean(nh_.get(), "nmea/gnssToFilter/qzss", false); - declareRosBoolean(nh_.get(), "nmea/gnssToFilter/glonass", false); - declareRosBoolean(nh_.get(), "nmea/gnssToFilter/beidou", false); + declareRosBoolean(this, "nmea.set", false); + declareRosBoolean(this, "nmea.compat", false); + declareRosBoolean(this, "nmea.consider", false); + declareRosBoolean(this, "nmea.limit82", false); + declareRosBoolean(this, "nmea.high_prec", false); + declareRosBoolean(this, "nmea.filter.pos", false); + declareRosBoolean(this, "nmea.filter.msk_pos", false); + declareRosBoolean(this, "nmea.filter.time", false); + declareRosBoolean(this, "nmea.filter.date", false); + declareRosBoolean(this, "nmea.filter.sbas", false); + declareRosBoolean(this, "nmea.filter.track", false); + declareRosBoolean(this, "nmea.filter.gps_only", false); + declareRosBoolean(this, "nmea.gnssToFilter.gps", false); + declareRosBoolean(this, "nmea.gnssToFilter.sbas", false); + declareRosBoolean(this, "nmea.gnssToFilter.qzss", false); + declareRosBoolean(this, "nmea.gnssToFilter.glonass", false); + declareRosBoolean(this, "nmea.gnssToFilter.beidou", false); // Publish parameters - declareRosBoolean(nh_.get(), "publish/all", false); - - declareRosBoolean(nh_.get(), "publish/nav/all", getRosBoolean(nh_.get(), "publish/all")); - declareRosBoolean(nh_.get(), "publish/nav/att", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/clock", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/heading", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/posecef", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/posllh", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/pvt", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/relposned", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/sat", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/sol", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/svin", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/svinfo", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/status", getRosBoolean(nh_.get(), "publish/nav/all")); - declareRosBoolean(nh_.get(), "publish/nav/velned", getRosBoolean(nh_.get(), "publish/nav/all")); - - declareRosBoolean(nh_.get(), "publish/rxm/all", getRosBoolean(nh_.get(), "publish/all")); - declareRosBoolean(nh_.get(), "publish/rxm/almRaw", getRosBoolean(nh_.get(), "publish/rxm/all")); - declareRosBoolean(nh_.get(), "publish/rxm/eph", getRosBoolean(nh_.get(), "publish/rxm/all")); - declareRosBoolean(nh_.get(), "publish/rxm/rtcm", getRosBoolean(nh_.get(), "publish/rxm/all")); - declareRosBoolean(nh_.get(), "publish/rxm/raw", getRosBoolean(nh_.get(), "publish/rxm/all")); - declareRosBoolean(nh_.get(), "publish/rxm/sfrb", getRosBoolean(nh_.get(), "publish/rxm/all")); - - declareRosBoolean(nh_.get(), "publish/aid/all", getRosBoolean(nh_.get(), "publish/all")); - declareRosBoolean(nh_.get(), "publish/aid/alm", getRosBoolean(nh_.get(), "publish/aid/all")); - declareRosBoolean(nh_.get(), "publish/aid/eph", getRosBoolean(nh_.get(), "publish/aid/all")); - declareRosBoolean(nh_.get(), "publish/aid/hui", getRosBoolean(nh_.get(), "publish/aid/all")); - - declareRosBoolean(nh_.get(), "publish/mon/all", getRosBoolean(nh_.get(), "publish/all")); - declareRosBoolean(nh_.get(), "publish/mon/hw", getRosBoolean(nh_.get(), "publish/mon/all")); - - declareRosBoolean(nh_.get(), "publish/tim/tm2", false); + declareRosBoolean(this, "publish.all", false); + + declareRosBoolean(this, "publish.nav.all", getRosBoolean(this, "publish.all")); + declareRosBoolean(this, "publish.nav.att", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.clock", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.heading", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.posecef", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.posllh", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.pvt", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.relposned", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.sat", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.sol", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.svin", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.svinfo", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.status", getRosBoolean(this, "publish.nav.all")); + declareRosBoolean(this, "publish.nav.velned", getRosBoolean(this, "publish.nav.all")); + + declareRosBoolean(this, "publish.rxm.all", getRosBoolean(this, "publish.all")); + declareRosBoolean(this, "publish.rxm.almRaw", getRosBoolean(this, "publish.rxm.all")); + declareRosBoolean(this, "publish.rxm.eph", getRosBoolean(this, "publish.rxm.all")); + declareRosBoolean(this, "publish.rxm.rtcm", getRosBoolean(this, "publish.rxm.all")); + declareRosBoolean(this, "publish.rxm.raw", getRosBoolean(this, "publish.rxm.all")); + declareRosBoolean(this, "publish.rxm.sfrb", getRosBoolean(this, "publish.rxm.all")); + + declareRosBoolean(this, "publish.aid.all", getRosBoolean(this, "publish.all")); + declareRosBoolean(this, "publish.aid.alm", getRosBoolean(this, "publish.aid.all")); + declareRosBoolean(this, "publish.aid.eph", getRosBoolean(this, "publish.aid.all")); + declareRosBoolean(this, "publish.aid.hui", getRosBoolean(this, "publish.aid.all")); + + declareRosBoolean(this, "publish.mon.all", getRosBoolean(this, "publish.all")); + declareRosBoolean(this, "publish.mon.hw", getRosBoolean(this, "publish.mon.all")); + + declareRosBoolean(this, "publish.tim.tm2", false); // INF parameters - declareRosBoolean(nh_.get(), "inf/all", true); - declareRosBoolean(nh_.get(), "inf/debug", false); - declareRosBoolean(nh_.get(), "inf/error", getRosBoolean(nh_.get(), "inf/all")); - declareRosBoolean(nh_.get(), "inf/notice", getRosBoolean(nh_.get(), "inf/all")); - declareRosBoolean(nh_.get(), "inf/test", getRosBoolean(nh_.get(), "inf/all")); - declareRosBoolean(nh_.get(), "inf/warning", getRosBoolean(nh_.get(), "inf/all")); + declareRosBoolean(this, "inf.all", true); + declareRosBoolean(this, "inf.debug", false); + declareRosBoolean(this, "inf.error", getRosBoolean(this, "inf.all")); + declareRosBoolean(this, "inf.notice", getRosBoolean(this, "inf.all")); + declareRosBoolean(this, "inf.test", getRosBoolean(this, "inf.all")); + declareRosBoolean(this, "inf.warning", getRosBoolean(this, "inf.all")); // ESF parameters - declareRosBoolean(nh_.get(), "publish/esf/all", true); - declareRosBoolean(nh_.get(), "publish/esf/ins", getRosBoolean(nh_.get(), "publish/esf/all")); - declareRosBoolean(nh_.get(), "publish/esf/meas", getRosBoolean(nh_.get(), "publish/esf/all")); - declareRosBoolean(nh_.get(), "publish/esf/raw", getRosBoolean(nh_.get(), "publish/esf/all")); - declareRosBoolean(nh_.get(), "publish/esf/status", getRosBoolean(nh_.get(), "publish/esf/all")); + declareRosBoolean(this, "publish.esf.all", true); + declareRosBoolean(this, "publish.esf.ins", getRosBoolean(this, "publish.esf.all")); + declareRosBoolean(this, "publish.esf.meas", getRosBoolean(this, "publish.esf.all")); + declareRosBoolean(this, "publish.esf.raw", getRosBoolean(this, "publish.esf.all")); + declareRosBoolean(this, "publish.esf.status", getRosBoolean(this, "publish.esf.all")); // HNR parameters - declareRosBoolean(nh_.get(), "publish/hnr/pvt", true); + declareRosBoolean(this, "publish.hnr.pvt", true); + + this->declare_parameter("tmode3", ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED); + std::vector empty_double; + std::vector empty_int; + this->declare_parameter("arp.position", empty_double); + this->declare_parameter("arp.position_hp", empty_int); + this->declare_parameter("arp.acc", 0.0); + this->declare_parameter("arp.lla_flag", false); } -void UbloxNode::pollMessages(const ros::TimerEvent& event) { +void UbloxNode::pollMessages() { static std::vector payload(1, 1); - if (getRosBoolean(nh_.get(), "publish/aid/alm")) { + if (getRosBoolean(this, "publish.aid.alm")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload); } - if (getRosBoolean(nh_.get(), "publish/aid/eph")) { + if (getRosBoolean(this, "publish.aid.eph")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload); } - if (getRosBoolean(nh_.get(), "publish/aid/hui")) { + if (getRosBoolean(this, "publish.aid.hui")) { gps_->poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI); } @@ -444,87 +453,87 @@ void UbloxNode::pollMessages(const ros::TimerEvent& event) { } } -void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) { +void UbloxNode::printInf(const ublox_msgs::msg::Inf &m, uint8_t id) { if (id == ublox_msgs::Message::INF::ERROR) { - ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); + // RCLCPP_ERROR_STREAM(this->get_logger(), "INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); } else if (id == ublox_msgs::Message::INF::WARNING) { - ROS_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); + // RCLCPP_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); } else if (id == ublox_msgs::Message::INF::DEBUG) { - ROS_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); + // RCLCPP_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); } else { - ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end())); + // RCLCPP_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); } } void UbloxNode::subscribe() { - ROS_DEBUG("Subscribing to U-Blox messages"); + RCLCPP_DEBUG(this->get_logger(), "Subscribing to U-Blox messages"); // subscribe messages // Nav Messages - if (getRosBoolean(nh_.get(), "publish/nav/status")) { - gps_->subscribe([this](const ublox_msgs::NavSTATUS &m) { nav_status_pub_.publish(m); }, + if (getRosBoolean(this, "publish.nav.status")) { + gps_->subscribe([this](const ublox_msgs::msg::NavSTATUS &m) { nav_status_pub_->publish(m); }, 1); } - if (getRosBoolean(nh_.get(), "publish/nav/posecef")) { - gps_->subscribe([this](const ublox_msgs::NavPOSECEF &m) { nav_posecef_pub_.publish(m); }, + if (getRosBoolean(this, "publish.nav.posecef")) { + gps_->subscribe([this](const ublox_msgs::msg::NavPOSECEF &m) { nav_posecef_pub_->publish(m); }, 1); } - if (getRosBoolean(nh_.get(), "publish/nav/clock")) { - gps_->subscribe([this](const ublox_msgs::NavCLOCK &m) { nav_clock_pub_.publish(m); }, + if (getRosBoolean(this, "publish.nav.clock")) { + gps_->subscribe([this](const ublox_msgs::msg::NavCLOCK &m) { nav_clock_pub_->publish(m); }, 1); } // INF messages - if (getRosBoolean(nh_.get(), "inf/debug")) { - gps_->subscribeId( + if (getRosBoolean(this, "inf.debug")) { + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); } - if (getRosBoolean(nh_.get(), "inf/error")) { - gps_->subscribeId( + if (getRosBoolean(this, "inf.error")) { + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); } - if (getRosBoolean(nh_.get(), "inf/notice")) { - gps_->subscribeId( + if (getRosBoolean(this, "inf.notice")) { + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); } - if (getRosBoolean(nh_.get(), "inf/test")) { - gps_->subscribeId( + if (getRosBoolean(this, "inf.test")) { + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); } - if (getRosBoolean(nh_.get(), "inf/warning")) { - gps_->subscribeId( + if (getRosBoolean(this, "inf.warning")) { + gps_->subscribeId( std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); } // AID messages - if (getRosBoolean(nh_.get(), "publish/aid/alm")) { - gps_->subscribe([this](const ublox_msgs::AidALM &m) { aid_alm_pub_.publish(m); }, + if (getRosBoolean(this, "publish.aid.alm")) { + gps_->subscribe([this](const ublox_msgs::msg::AidALM &m) { aid_alm_pub_->publish(m); }, 1); } - if (getRosBoolean(nh_.get(), "publish/aid/eph")) { - gps_->subscribe([this](const ublox_msgs::AidEPH &m) { aid_eph_pub_.publish(m); }, + if (getRosBoolean(this, "publish.aid.eph")) { + gps_->subscribe([this](const ublox_msgs::msg::AidEPH &m) { aid_eph_pub_->publish(m); }, 1); } - if (getRosBoolean(nh_.get(), "publish/aid/hui")) { - gps_->subscribe([this](const ublox_msgs::AidHUI &m) { aid_hui_pub_.publish(m); }, + if (getRosBoolean(this, "publish.aid.hui")) { + gps_->subscribe([this](const ublox_msgs::msg::AidHUI &m) { aid_hui_pub_->publish(m); }, 1); } @@ -534,26 +543,26 @@ void UbloxNode::subscribe() { } void UbloxNode::initializeRosDiagnostics() { - declareRosBoolean(nh_.get(), "diagnostic_period", kDiagnosticPeriod); + declareRosBoolean(this, "diagnostic_period", kDiagnosticPeriod); - for (int i = 0; i < components_.size(); i++) { + for (size_t i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } } void UbloxNode::processMonVer() { - ublox_msgs::MonVER monVer; + ublox_msgs::msg::MonVER monVer; if (!gps_->poll(monVer)) { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); } - ROS_DEBUG("%s, HW VER: %s", monVer.sw_version.c_array(), - monVer.hw_version.c_array()); + // RCLCPP_DEBUG(this->get_logger(), "%s, HW VER: %s", monVer.sw_version.array(), + // monVer.hw_version.array()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); for (std::size_t i = 0; i < monVer.extension.size(); ++i) { - ROS_DEBUG("%s", monVer.extension[i].field.c_array()); + // RCLCPP_DEBUG(this->get_logger(), "%s", monVer.extension[i].field.array()); // Find the end of the string (null character) unsigned char* end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); @@ -570,7 +579,7 @@ void UbloxNode::processMonVer() { } } if (protocol_version_ == 0) { - ROS_WARN("Failed to parse MonVER and determine protocol version. %s", + RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); } addFirmwareInterface(); @@ -618,13 +627,13 @@ bool UbloxNode::configureUblox() { throw std::runtime_error("Failed to initialize."); } if (load_.load_mask != 0) { - ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.load_mask); + RCLCPP_DEBUG(this->get_logger(), "Loading u-blox configuration from memory. %u", load_.load_mask); if (!gps_->configure(load_)) { throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); } if (load_.load_mask & load_.MASK_IO_PORT) { - ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s", + RCLCPP_DEBUG(this->get_logger(), "Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); gps_->reset(wait); @@ -634,7 +643,7 @@ bool UbloxNode::configureUblox() { } } - if (getRosBoolean(nh_.get(), "config_on_startup")) { + if (getRosBoolean(this, "config_on_startup")) { if (set_usb_) { gps_->configUsb(usb_tx_, usb_in_, usb_out_); } @@ -646,15 +655,15 @@ bool UbloxNode::configureUblox() { } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { - if (!gps_->configSbas(getRosBoolean(nh_.get(), "gnss/sbas"), sbas_usage_, max_sbas_)) { + if (!gps_->configSbas(getRosBoolean(this, "gnss.sbas"), sbas_usage_, max_sbas_)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(nh_.get(), "gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(this, "gnss.sbas") ? "enable" : "disable") + " SBAS."); } } - if (!gps_->setPpp(getRosBoolean(nh_.get(), "enable_ppp"))) { + if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"))) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(nh_.get(), "enable_ppp") ? "enable" : "disable") + (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") + " PPP."); } if (!gps_->setDynamicModel(dmodel_)) { @@ -668,41 +677,41 @@ bool UbloxNode::configureUblox() { ss << "Failed to set dead reckoning limit: " << dr_limit_ << "."; throw std::runtime_error(ss.str()); } - if (getRosBoolean(nh_.get(), "dat/set") && !gps_->configure(cfg_dat_)) { + if (getRosBoolean(this, "dat.set") && !gps_->configure(cfg_dat_)) { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component - for (int i = 0; i < components_.size(); i++) { + for (size_t i = 0; i < components_.size(); i++) { if (!components_[i]->configureUblox(gps_)) { return false; } } } if (save_.save_mask != 0) { - ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u", + RCLCPP_DEBUG(this->get_logger(), "Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); if (!gps_->configure(save_)) { - ROS_ERROR("u-blox unable to save configuration to non-volatile memory"); + RCLCPP_ERROR(this->get_logger(), "u-blox unable to save configuration to non-volatile memory"); } } } catch (std::exception& e) { - ROS_FATAL("Error configuring u-blox: %s", e.what()); + RCLCPP_FATAL(this->get_logger(), "Error configuring u-blox: %s", e.what()); return false; } return true; } void UbloxNode::configureInf() { - ublox_msgs::CfgINF msg; + ublox_msgs::msg::CfgINF msg; // Subscribe to UBX INF messages - ublox_msgs::CfgINFBlock block; + ublox_msgs::msg::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (getRosBoolean(nh_.get(), "inf/error") ? block.INF_MSG_ERROR : 0) | - (getRosBoolean(nh_.get(), "inf/warning") ? block.INF_MSG_WARNING : 0) | - (getRosBoolean(nh_.get(), "inf/notice") ? block.INF_MSG_NOTICE : 0) | - (getRosBoolean(nh_.get(), "inf/test") ? block.INF_MSG_TEST : 0) | - (getRosBoolean(nh_.get(), "inf/debug") ? block.INF_MSG_DEBUG : 0); + uint8_t mask = (getRosBoolean(this, "inf.error") ? block.INF_MSG_ERROR : 0) | + (getRosBoolean(this, "inf.warning") ? block.INF_MSG_WARNING : 0) | + (getRosBoolean(this, "inf.notice") ? block.INF_MSG_NOTICE : 0) | + (getRosBoolean(this, "inf.test") ? block.INF_MSG_TEST : 0) | + (getRosBoolean(this, "inf.debug") ? block.INF_MSG_DEBUG : 0); for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { block.inf_msg_mask[i] = mask; } @@ -710,8 +719,8 @@ void UbloxNode::configureInf() { msg.blocks.push_back(block); // IF NMEA is enabled - if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) { - ublox_msgs::CfgINFBlock block; + if (uart_in_ & ublox_msgs::msg::CfgPRT::PROTO_NMEA) { + ublox_msgs::msg::CfgINFBlock block; block.protocol_id = block.PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { @@ -720,14 +729,14 @@ void UbloxNode::configureInf() { msg.blocks.push_back(block); } - ROS_DEBUG("Configuring INF messages"); + RCLCPP_DEBUG(this->get_logger(), "Configuring INF messages"); if (!gps_->configure(msg)) { - ROS_WARN("Failed to configure INF messages"); + RCLCPP_WARN(this->get_logger(), "Failed to configure INF messages"); } } void UbloxNode::initializeIo() { - gps_->setConfigOnStartup(getRosBoolean(nh_.get(), "config_on_startup")); + gps_->setConfigOnStartup(getRosBoolean(this, "config_on_startup")); std::smatch match; if (std::regex_match(device_, match, @@ -736,7 +745,7 @@ void UbloxNode::initializeIo() { if (proto == "tcp") { std::string host(match[2]); std::string port(match[3]); - ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps_->initializeTcp(host, port); } else { @@ -761,8 +770,8 @@ void UbloxNode::initialize() { // Must process Mon VER before setting firmware/hardware params processMonVer(); if (protocol_version_ <= 14) { - if (getRosBoolean(nh_.get(), "raw_data")) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, nh_.get())); + if (getRosBoolean(this, "raw_data")) { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, this)); } } // Must set firmware & hardware params before initializing diagnostics @@ -773,22 +782,21 @@ void UbloxNode::initialize() { initializeRosDiagnostics(); if (configureUblox()) { - ROS_INFO("U-Blox configured successfully."); + RCLCPP_INFO(this->get_logger(), "U-Blox configured successfully."); // Subscribe to all U-Blox messages subscribe(); // Configure INF messages (needs INF params, call after subscribing) configureInf(); - poller_ = nh_->createTimer(ros::Duration(kPollDuration), - &UbloxNode::pollMessages, - this); + poller_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(kPollDuration * 1000.0)), + std::bind(&UbloxNode::pollMessages, this)); } } void UbloxNode::shutdown() { if (gps_->isInitialized()) { gps_->close(); - ROS_INFO("Closed connection to %s.", device_.c_str()); + RCLCPP_INFO(this->get_logger(), "Closed connection to %s.", device_.c_str()); } } diff --git a/ublox_gps/src/node_main.cpp b/ublox_gps/src/node_main.cpp index dac05883..7363d40a 100644 --- a/ublox_gps/src/node_main.cpp +++ b/ublox_gps/src/node_main.cpp @@ -1,12 +1,20 @@ -#include +#include + +#include #include int main(int argc, char** argv) { - ros::init(argc, argv, "ublox_gps"); - ublox_node::UbloxNode node; + // Force flush of the stdout buffer. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); - ros::spin(); + rclcpp::shutdown(); return 0; } diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 86792d08..071cdda7 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -32,7 +32,7 @@ // the data as ros messages. This is used by our group to also evaluate the // measured data with the rtklib. -#include "ublox_gps/raw_data_pa.hpp" +#include #include #include #include @@ -41,27 +41,37 @@ #include #include +#include + +#include +#include + +#include + // // ublox_node namespace // namespace ublox_node { -RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : - pnh_(ros::NodeHandle("~")), +RawDataStreamPa::RawDataStreamPa(bool is_ros_subscriber) : rclcpp::Node("raw_data_pa"), flag_publish_(false), is_ros_subscriber_(is_ros_subscriber) { - raw_pub_ = pnh_.advertise("raw_data_stream", 100); + raw_pub_ = this->create_publisher("raw_data_stream", 100); + + this->declare_parameter("dir", ""); + this->declare_parameter("raw_data_stream.dir", ""); + this->declare_parameter("raw_data_stream.publish", false); } void RawDataStreamPa::getRosParams() { if (is_ros_subscriber_) { - pnh_.param("dir", file_dir_, ""); + file_dir_ = this->get_parameter("dir").get_value(); } else { - pnh_.param("raw_data_stream/dir", file_dir_, ""); - pnh_.param("raw_data_stream/publish", flag_publish_, false); + file_dir_ = this->get_parameter("raw_data_stream.dir").get_value(); + flag_publish_ = this->get_parameter("raw_data_stream.publish").get_value(); } } @@ -77,23 +87,23 @@ bool RawDataStreamPa::isEnabled() { void RawDataStreamPa::initialize() { if (is_ros_subscriber_) { - ROS_INFO("Subscribing to raw data stream."); + RCLCPP_INFO(this->get_logger(), "Subscribing to raw data stream."); raw_data_stream_sub_ = - nh_.subscribe("raw_data_stream", 100, - &RawDataStreamPa::msgCallback, this); + this->create_subscription("raw_data_stream", rclcpp::QoS(100), + std::bind(&RawDataStreamPa::msgCallback, this, std::placeholders::_1)); } else if (flag_publish_) { - ROS_INFO("Publishing raw data stream."); + RCLCPP_INFO(this->get_logger(), "Publishing raw data stream."); RawDataStreamPa::publishMsg(std::string()); } if (!file_dir_.empty()) { struct stat stat_info; - if (stat(file_dir_.c_str(), &stat_info ) != 0) { - ROS_ERROR("Can't log raw data to file. " + if (::stat(file_dir_.c_str(), &stat_info ) != 0) { + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " "Directory \"%s\" does not exist.", file_dir_.c_str()); } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { - ROS_ERROR("Can't log raw data to file. " + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " "\"%s\" exists, but is not a directory.", file_dir_.c_str()); } else { @@ -123,10 +133,10 @@ void RawDataStreamPa::initialize() { try { file_handle_.open(file_name_); - ROS_INFO("Logging raw data to file \"%s\"", + RCLCPP_INFO(this->get_logger(), "Logging raw data to file \"%s\"", file_name_.c_str()); } catch(const std::exception& e) { - ROS_ERROR("Can't log raw data to file. " + RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " "Can't create file \"%s\".", file_name_.c_str()); } } @@ -146,20 +156,20 @@ void RawDataStreamPa::ubloxCallback(const unsigned char* data, } void RawDataStreamPa::msgCallback( - const std_msgs::UInt8MultiArray::ConstPtr& msg) { + const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { std::string str(msg->data.size(), ' '); std::copy(msg->data.begin(), msg->data.end(), str.begin()); saveToFile(str); } -std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( +std_msgs::msg::UInt8MultiArray RawDataStreamPa::str2uint8( const std::string & str) { - std_msgs::UInt8MultiArray msg; + std_msgs::msg::UInt8MultiArray msg; msg.layout.data_offset = 0; - msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); + msg.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); msg.layout.dim[0].size = str.length(); msg.layout.dim[0].stride = 1; msg.layout.dim[0].label = "raw_data_stream"; @@ -172,7 +182,7 @@ std_msgs::UInt8MultiArray RawDataStreamPa::str2uint8( void RawDataStreamPa::publishMsg(const std::string & str) { - raw_pub_.publish(RawDataStreamPa::str2uint8(str)); + raw_pub_->publish(RawDataStreamPa::str2uint8(str)); } void RawDataStreamPa::saveToFile(const std::string & str) { @@ -182,7 +192,7 @@ void RawDataStreamPa::saveToFile(const std::string & str) { file_handle_ << str; // file_handle_.flush(); } catch(const std::exception& e) { - ROS_WARN("Error writing to file \"%s\"", file_name_.c_str()); + RCLCPP_WARN(this->get_logger(), "Error writing to file \"%s\"", file_name_.c_str()); } } } diff --git a/ublox_gps/src/raw_data_product.cpp b/ublox_gps/src/raw_data_product.cpp index 1db9d167..a2887eb2 100644 --- a/ublox_gps/src/raw_data_product.cpp +++ b/ublox_gps/src/raw_data_product.cpp @@ -1,12 +1,12 @@ #include -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -18,54 +18,54 @@ namespace ublox_node { // // Raw Data Products // -RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, ros::NodeHandle* node) +RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node) : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { - rxm_raw_pub_ = node_->advertise("rxmraw", 1); - rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); - rxm_eph_pub_ = node_->advertise("rxmeph", 1); - rxm_alm_pub_ = node_->advertise("rxmalm", 1); + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); + rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); } void RawDataProduct::subscribe(std::shared_ptr gps) { // Subscribe to RXM Raw - if (getRosBoolean(node_, "publish/rxm/raw")) { - gps->subscribe([this](const ublox_msgs::RxmRAW &m) { rxm_raw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.raw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRAW &m) { rxm_raw_pub_->publish(m); }, 1); } // Subscribe to RXM SFRB - if (getRosBoolean(node_, "publish/rxm/sfrb")) { - gps->subscribe([this](const ublox_msgs::RxmSFRB &m) { rxm_sfrb_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + gps->subscribe([this](const ublox_msgs::msg::RxmSFRB &m) { rxm_sfrb_pub_->publish(m); }, 1); } // Subscribe to RXM EPH - if (getRosBoolean(node_, "publish/rxm/eph")) { - gps->subscribe([this](const ublox_msgs::RxmEPH &m) { rxm_eph_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.eph")) { + gps->subscribe([this](const ublox_msgs::msg::RxmEPH &m) { rxm_eph_pub_->publish(m); }, 1); } // Subscribe to RXM ALM - if (getRosBoolean(node_, "publish/rxm/almRaw")) { - gps->subscribe([this](const ublox_msgs::RxmALM &m) { rxm_alm_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.almRaw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmALM &m) { rxm_alm_pub_->publish(m); }, 1); } } void RawDataProduct::initializeRosDiagnostics() { - if (getRosBoolean(node_, "publish/rxm/raw")) { + if (getRosBoolean(node_, "publish.rxm.raw")) { freq_diagnostics_.push_back(std::make_shared( "rxmraw", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(node_, "publish/rxm/sfrb")) { + if (getRosBoolean(node_, "publish.rxm.sfrb")) { freq_diagnostics_.push_back(std::make_shared( "rxmsfrb", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(node_, "publish/rxm/eph")) { + if (getRosBoolean(node_, "publish.rxm.eph")) { freq_diagnostics_.push_back(std::make_shared( "rxmeph", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } - if (getRosBoolean(node_, "publish/rxm/almRaw")) { + if (getRosBoolean(node_, "publish.rxm.almRaw")) { freq_diagnostics_.push_back(std::make_shared( "rxmalm", kRtcmFreqTol, kRtcmFreqWindow, nav_rate_, meas_rate_, updater_)); } diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp index 942bf6d8..4f705cc4 100644 --- a/ublox_gps/src/tim_product.cpp +++ b/ublox_gps/src/tim_product.cpp @@ -4,12 +4,12 @@ #include #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include #include #include @@ -20,14 +20,14 @@ namespace ublox_node { // // U-Blox Time Sync Products, partially implemented. // -TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, ros::NodeHandle* node) : frame_id_(frame_id), updater_(updater), node_(node) +TimProduct::TimProduct(const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : frame_id_(frame_id), updater_(updater), node_(node) { timtm2_pub_ = - node_->advertise("timtm2", 1); + node_->create_publisher("timtm2", 1); interrupt_time_pub_ = - node_->advertise("interrupt_time", 1); - rxm_sfrb_pub_ = node_->advertise("rxmsfrb", 1); - rxm_raw_pub_ = node_->advertise("rxmraw", 1); + node_->create_publisher("interrupt_time", 1); + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); } void TimProduct::getRosParams() { @@ -48,42 +48,41 @@ bool TimProduct::configureUblox(std::shared_ptr gps) { } void TimProduct::subscribe(std::shared_ptr gps) { - gps->subscribe(std::bind( + gps->subscribe(std::bind( &TimProduct::callbackTimTM2, this, std::placeholders::_1), 1); - ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); + // RCLCPP_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2"); // Subscribe to SFRBX messages - if (getRosBoolean(node_, "publish/rxm/sfrb")) { - gps->subscribe([this](const ublox_msgs::RxmSFRBX &m) { rxm_sfrb_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + gps->subscribe([this](const ublox_msgs::msg::RxmSFRBX &m) { rxm_sfrb_pub_->publish(m); }, 1); } // Subscribe to RawX messages - if (getRosBoolean(node_, "publish/rxm/raw")) { - gps->subscribe([this](const ublox_msgs::RxmRAWX &m) { rxm_raw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.raw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRAWX &m) { rxm_raw_pub_->publish(m); }, 1); } } -void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) { - if (getRosBoolean(node_, "publish/tim/tm2")) { +void TimProduct::callbackTimTM2(const ublox_msgs::msg::TimTM2 &m) { + if (getRosBoolean(node_, "publish.tim.tm2")) { // create time ref message and put in the data - t_ref_.header.seq = m.rising_edge_count; - t_ref_.header.stamp = ros::Time::now(); + t_ref_.header.stamp = node_->now(); t_ref_.header.frame_id = frame_id_; - t_ref_.time_ref = ros::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); + t_ref_.time_ref = rclcpp::Time((m.wn_r * 604800 + m.tow_ms_r / 1000), (m.tow_ms_r % 1000) * 1000000 + m.tow_sub_ms_r); std::ostringstream src; src << "TIM" << int(m.ch); t_ref_.source = src.str(); - t_ref_.header.stamp = ros::Time::now(); // create a new timestamp + t_ref_.header.stamp = node_->now(); // create a new timestamp t_ref_.header.frame_id = frame_id_; - timtm2_pub_.publish(m); - interrupt_time_pub_.publish(t_ref_); + timtm2_pub_->publish(m); + interrupt_time_pub_->publish(t_ref_); } updater_->force_update(); diff --git a/ublox_gps/src/ublox_firmware.cpp b/ublox_gps/src/ublox_firmware.cpp index 85c0ae99..ab273e31 100644 --- a/ublox_gps/src/ublox_firmware.cpp +++ b/ublox_gps/src/ublox_firmware.cpp @@ -1,7 +1,7 @@ #include -#include -#include +#include +#include #include #include @@ -11,7 +11,7 @@ namespace ublox_node { // // U-Blox Firmware (all versions) // -UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, ros::NodeHandle* node) : updater_(updater), gnss_(gnss), node_(node) +UbloxFirmware::UbloxFirmware(std::shared_ptr updater, std::shared_ptr gnss, rclcpp::Node* node) : updater_(updater), gnss_(gnss), node_(node) { } diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index c0206ea5..4ba33454 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -3,18 +3,18 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include #include #include @@ -27,65 +27,63 @@ namespace ublox_node { // // U-Blox Firmware Version 6 // -UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) +UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { nav_pos_llh_pub_ = - node_->advertise("navposllh", 1); + node_->create_publisher("navposllh", 1); fix_pub_ = - node_->advertise("fix", 1); + node_->create_publisher("fix", 1); nav_vel_ned_pub_ = - node_->advertise("navvelned", 1); + node_->create_publisher("navvelned", 1); vel_pub_ = - node_->advertise("fix_velocity", + node_->create_publisher("fix_velocity", 1); nav_sol_pub_ = - node_->advertise("navsol", 1); + node_->create_publisher("navsol", 1); nav_svinfo_pub_ = - node_->advertise("navinfo", 1); + node_->create_publisher("navinfo", 1); mon_hw_pub_ = - node_->advertise("monhw", 1); + node_->create_publisher("monhw", 1); } void UbloxFirmware6::getRosParams() { // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - if (getRosBoolean(node_, "nmea/set")) { - bool compat, consider; - - if (!getRosUint(node_, "nmea/version", cfg_nmea_.version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + if (getRosBoolean(node_, "nmea.set")) { + if (!getRosUint(node_, "nmea.version", cfg_nmea_.version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); } // set flags - cfg_nmea_.flags = getRosBoolean(node_, "nmea/compat") ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = getRosBoolean(node_, "nmea.compat") ? cfg_nmea_.FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; } } bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { - ROS_WARN("ublox_version < 7, ignoring GNSS settings"); + RCLCPP_WARN(node_->get_logger(), "ublox_version < 7, ignoring GNSS settings"); - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -95,24 +93,24 @@ bool UbloxFirmware6::configureUblox(std::shared_ptr gps) { void UbloxFirmware6::subscribe(std::shared_ptr gps) { // Always subscribes to these messages, but may not publish to ROS topic // Subscribe to Nav POSLLH - gps->subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware6::callbackNavPosLlh, this, std::placeholders::_1), 1); - gps->subscribe(std::bind( + gps->subscribe(std::bind( // Subscribe to Nav SOL &UbloxFirmware6::callbackNavSol, this, std::placeholders::_1), 1); // Subscribe to Nav VELNED - gps->subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware6::callbackNavVelNed, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean(node_, "publish/nav/svinfo")) { - gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.nav.svinfo")) { + gps->subscribe([this](const ublox_msgs::msg::NavSVINFO &m) { nav_svinfo_pub_->publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW6 &m) { mon_hw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW6 &m) { mon_hw_pub_->publish(m); }, 1); } } @@ -120,31 +118,31 @@ void UbloxFirmware6::subscribe(std::shared_ptr gps) { void UbloxFirmware6::fixDiagnostic( diagnostic_updater::DiagnosticStatusWrapper& stat) { // Set the diagnostic level based on the fix status - if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_DEAD_RECKONING_ONLY) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Dead reckoning only"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_2D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_2D_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "2D fix"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_3D_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_3D_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "3D fix"; } else if (last_nav_sol_.gps_fix == - ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + ublox_msgs::msg::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "GPS and dead reckoning combined"; - } else if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::OK; + } else if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_TIME_ONLY_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Time fix only"; } // If fix is not ok (within DOP & Accuracy Masks), raise the diagnostic level - if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) { - stat.level = diagnostic_msgs::DiagnosticStatus::WARN; + if (!(last_nav_sol_.flags & ublox_msgs::msg::NavSOL::FLAGS_GPS_FIX_OK)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message += ", fix not ok"; } // Raise diagnostic level to error if no fix - if (last_nav_sol_.gps_fix == ublox_msgs::NavSOL::GPS_NO_FIX) { - stat.level = diagnostic_msgs::DiagnosticStatus::ERROR; + if (last_nav_sol_.gps_fix == ublox_msgs::msg::NavSOL::GPS_NO_FIX) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat.message = "No fix"; } @@ -159,16 +157,16 @@ void UbloxFirmware6::fixDiagnostic( stat.add("# SVs used", (int)last_nav_sol_.num_sv); } -void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { - if (getRosBoolean(node_, "publish/nav/posllh")) { - nav_pos_llh_pub_.publish(m); +void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { + if (getRosBoolean(node_, "publish.nav.posllh")) { + nav_pos_llh_pub_->publish(m); } // Position message if (m.i_tow == last_nav_vel_.i_tow) { fix_.header.stamp = velocity_.header.stamp; // use last timestamp } else { - fix_.header.stamp = ros::Time::now(); // new timestamp + fix_.header.stamp = node_->now(); // new timestamp } fix_.header.frame_id = frame_id_; @@ -190,26 +188,26 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) { fix_.position_covariance[4] = var_h; fix_.position_covariance[8] = var_v; fix_.position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; fix_.status.service = fix_.status.SERVICE_GPS; - fix_pub_.publish(fix_); + fix_pub_->publish(fix_); last_nav_pos_ = m; // update diagnostics freq_diag_->diagnostic->tick(fix_.header.stamp); - updater_->update(); + updater_->force_update(); } -void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { - if (getRosBoolean(node_, "publish/nav/velned")) { - nav_vel_ned_pub_.publish(m); +void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::msg::NavVELNED& m) { + if (getRosBoolean(node_, "publish.nav.velned")) { + nav_vel_ned_pub_->publish(m); } // Example geometry message if (m.i_tow == last_nav_pos_.i_tow) { velocity_.header.stamp = fix_.header.stamp; // same time as last navposllh } else { - velocity_.header.stamp = ros::Time::now(); // create a new timestamp + velocity_.header.stamp = node_->now(); // create a new timestamp } velocity_.header.frame_id = frame_id_; @@ -226,13 +224,13 @@ void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) { velocity_.twist.covariance[cols * 2 + 2] = var_speed; velocity_.twist.covariance[cols * 3 + 3] = -1; // angular rate unsupported - vel_pub_.publish(velocity_); + vel_pub_->publish(velocity_); last_nav_vel_ = m; } -void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) { - if (getRosBoolean(node_, "publish/nav/sol")) { - nav_sol_pub_.publish(m); +void UbloxFirmware6::callbackNavSol(const ublox_msgs::msg::NavSOL& m) { + if (getRosBoolean(node_, "publish.nav.sol")) { + nav_sol_pub_->publish(m); } last_nav_sol_ = m; } diff --git a/ublox_gps/src/ublox_firmware7.cpp b/ublox_gps/src/ublox_firmware7.cpp index c81807fc..00ecfc84 100644 --- a/ublox_gps/src/ublox_firmware7.cpp +++ b/ublox_gps/src/ublox_firmware7.cpp @@ -1,13 +1,15 @@ #include #include -#include +#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include #include #include @@ -22,101 +24,101 @@ void UbloxFirmware7::getRosParams() { // GNSS configuration // // GNSS enable/disable - enable_gps_ = getRosBoolean(node_, "gnss/gps"); - enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); - enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); + enable_gps_ = getRosBoolean(node_, "gnss.gps"); + enable_glonass_ = getRosBoolean(node_, "gnss.glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss.qzss"); - getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + getRosUint(node_, "gnss.qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device"); + RCLCPP_WARN(node_->get_logger(), "gnss/gps is true, but GPS GNSS is not supported by this device"); } if (enable_glonass_ && !gnss_->isSupported("GLO")) { - ROS_WARN("gnss/glonass is true, but GLONASS is not %s", + RCLCPP_WARN(node_->get_logger(), "gnss/glonass is true, but GLONASS is not %s", "supported by this device"); } if (enable_qzss_ && !gnss_->isSupported("QZSS")) { - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); + RCLCPP_WARN(node_->get_logger(), "gnss/qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + if (getRosBoolean(node_, "gnss.sbas") && !gnss_->isSupported("SBAS")) { + RCLCPP_WARN(node_->get_logger(), "gnss/sbas is true, but SBAS is not supported by this device"); } - if (getRosBoolean(node_, "gnss/galileo")) { - ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings"); + if (getRosBoolean(node_, "gnss.galileo")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring Galileo GNSS Settings"); } - if (getRosBoolean(node_, "gnss/beidou")) { - ROS_WARN("ublox_version < 8, ignoring BeiDou Settings"); + if (getRosBoolean(node_, "gnss.beidou")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring BeiDou Settings"); } - if (getRosBoolean(node_, "gnss/imes")) { - ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings"); + if (getRosBoolean(node_, "gnss.imes")) { + RCLCPP_WARN(node_->get_logger(), "ublox_version < 8, ignoring IMES GNSS Settings"); } // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS; + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS; // // NMEA Configuration // - if (getRosBoolean(node_, "nmea/set")) { + if (getRosBoolean(node_, "nmea.set")) { bool compat, consider; - if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + if (!getRosUint(node_, "nmea.version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); } - if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + if (!getRosUint(node_, "nmea.sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.sv_numbering must be set"); } - if (!node_->getParam("nmea/compat", compat)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + if (!node_->get_parameter("nmea.compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.compat must be set"); } - if (!node_->getParam("nmea/consider", consider)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + if (!node_->get_parameter("nmea.consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.consider must be set"); } // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); } } bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { /** Configure the GNSS **/ - ublox_msgs::CfgGNSS cfgGNSSRead; + ublox_msgs::msg::CfgGNSS cfgGNSSRead; if (gps->poll(cfgGNSSRead)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); - ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfgGNSSRead.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfgGNSSRead.num_trk_ch_use); } else { throw std::runtime_error("Failed to read the GNSS config."); } - ublox_msgs::CfgGNSS cfgGNSSWrite; + ublox_msgs::msg::CfgGNSS cfgGNSSWrite; cfgGNSSWrite.num_config_blocks = 1; // do services one by one cfgGNSSWrite.num_trk_ch_hw = cfgGNSSRead.num_trk_ch_hw; cfgGNSSWrite.num_trk_ch_use = cfgGNSSRead.num_trk_ch_use; @@ -124,7 +126,7 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { // configure GLONASS if (gnss_->isSupported("GLO")) { - ublox_msgs::CfgGNSSBlock block; + ublox_msgs::msg::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_GLONASS; block.res_trk_ch = block.RES_TRK_CH_GLONASS; block.max_trk_ch = block.MAX_TRK_CH_GLONASS; @@ -139,7 +141,7 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { if (gnss_->isSupported("QZSS")) { // configure QZSS - ublox_msgs::CfgGNSSBlock block; + ublox_msgs::msg::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_QZSS; block.res_trk_ch = block.RES_TRK_CH_QZSS; block.max_trk_ch = block.MAX_TRK_CH_QZSS; @@ -154,20 +156,20 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { if (gnss_->isSupported("SBAS")) { // configure SBAS - ublox_msgs::CfgGNSSBlock block; + ublox_msgs::msg::CfgGNSSBlock block; block.gnss_id = block.GNSS_ID_SBAS; block.res_trk_ch = block.RES_TRK_CH_SBAS; block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = getRosBoolean(node_, "gnss/sbas") ? block.SIG_CFG_SBAS_L1CA : 0; + block.flags = getRosBoolean(node_, "gnss.sbas") ? block.SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(node_, "gnss/sbas") ? "enable" : "disable") + + (getRosBoolean(node_, "gnss.sbas") ? "enable" : "disable") + " SBAS."); } } - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -177,19 +179,19 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { void UbloxFirmware7::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT (always does so since fix information is published // from this) - gps->subscribe(std::bind( + gps->subscribe(std::bind( &UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SVINFO - if (getRosBoolean(node_, "publish/nav/svinfo")) { - gps->subscribe([this](const ublox_msgs::NavSVINFO &m) { nav_svinfo_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.nav.svinfo")) { + gps->subscribe([this](const ublox_msgs::msg::NavSVINFO &m) { nav_svinfo_pub_->publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW &m) { mon_hw_pub_->publish(m); }, 1); } } diff --git a/ublox_gps/src/ublox_firmware8.cpp b/ublox_gps/src/ublox_firmware8.cpp index b91ad0ed..e1b80759 100644 --- a/ublox_gps/src/ublox_firmware8.cpp +++ b/ublox_gps/src/ublox_firmware8.cpp @@ -5,13 +5,13 @@ #include #include -#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -27,103 +27,103 @@ void UbloxFirmware8::getRosParams() { save_on_shutdown_ = getRosBoolean(node_, "save_on_shutdown"); // GNSS enable/disable - enable_gps_ = getRosBoolean(node_, "gnss/gps"); - enable_galileo_ = getRosBoolean(node_, "gnss/galileo"); - enable_beidou_ = getRosBoolean(node_, "gnss/beidou"); - enable_imes_ = getRosBoolean(node_, "gnss/imes"); - enable_glonass_ = getRosBoolean(node_, "gnss/glonass"); - enable_qzss_ = getRosBoolean(node_, "gnss/qzss"); + enable_gps_ = getRosBoolean(node_, "gnss.gps"); + enable_galileo_ = getRosBoolean(node_, "gnss.galileo"); + enable_beidou_ = getRosBoolean(node_, "gnss.beidou"); + enable_imes_ = getRosBoolean(node_, "gnss.imes"); + enable_glonass_ = getRosBoolean(node_, "gnss.glonass"); + enable_qzss_ = getRosBoolean(node_, "gnss.qzss"); // QZSS Signal Configuration - getRosUint(node_, "gnss/qzss_sig_cfg", qzss_sig_cfg_, - ublox_msgs::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); + getRosUint(node_, "gnss.qzss_sig_cfg", qzss_sig_cfg_, + ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_QZSS_L1CA); if (enable_gps_ && !gnss_->isSupported("GPS")) { - ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s", + RCLCPP_WARN(node_->get_logger(), "gnss.gps is true, but GPS GNSS is not supported by %s", "this device"); } if (enable_glonass_ && !gnss_->isSupported("GLO")) { - ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s", + RCLCPP_WARN(node_->get_logger(), "gnss.glonass is true, but GLONASS is not supported by %s", "this device"); } if (enable_galileo_ && !gnss_->isSupported("GAL")) { - ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s", + RCLCPP_WARN(node_->get_logger(), "gnss.galileo is true, but Galileo GNSS is not supported %s", "by this device"); } if (enable_beidou_ && !gnss_->isSupported("BDS")) { - ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s", + RCLCPP_WARN(node_->get_logger(), "gnss.beidou is true, but Beidou GNSS is not supported %s", "by this device"); } if (enable_imes_ && !gnss_->isSupported("IMES")) { - ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s", + RCLCPP_WARN(node_->get_logger(), "gnss.imes is true, but IMES GNSS is not supported by %s", "this device"); } if (enable_qzss_ && !gnss_->isSupported("QZSS")) { - ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device"); + RCLCPP_WARN(node_->get_logger(), "gnss.qzss is true, but QZSS is not supported by this device"); } - if (getRosBoolean(node_, "gnss/sbas") && !gnss_->isSupported("SBAS")) { - ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device"); + if (getRosBoolean(node_, "gnss.sbas") && !gnss_->isSupported("SBAS")) { + RCLCPP_WARN(node_->get_logger(), "gnss.sbas is true, but SBAS is not supported by this device"); } // Fix Service type, used when publishing fix status messages - fix_status_service_ = sensor_msgs::NavSatStatus::SERVICE_GPS - + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS - + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS - + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO; + fix_status_service_ = sensor_msgs::msg::NavSatStatus::SERVICE_GPS + + (enable_glonass_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS + + (enable_beidou_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_COMPASS + + (enable_galileo_ ? 1 : 0) * sensor_msgs::msg::NavSatStatus::SERVICE_GALILEO; // // NMEA Configuration // - if (getRosBoolean(node_, "nmea/set")) { + if (getRosBoolean(node_, "nmea.set")) { bool compat, consider; cfg_nmea_.version = cfg_nmea_.VERSION; // message version // Verify that parameters are set - if (!getRosUint(node_, "nmea/version", cfg_nmea_.nmea_version)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/version must be set"); + if (!getRosUint(node_, "nmea.version", cfg_nmea_.nmea_version)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.version must be set"); } - if (!getRosUint(node_, "nmea/num_sv", cfg_nmea_.num_sv)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/num_sv must be set"); + if (!getRosUint(node_, "nmea.num_sv", cfg_nmea_.num_sv)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.num_sv must be set"); } - if (!getRosUint(node_, "nmea/sv_numbering", cfg_nmea_.sv_numbering)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/sv_numbering must be set"); + if (!getRosUint(node_, "nmea.sv_numbering", cfg_nmea_.sv_numbering)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.sv_numbering must be set"); } - if (!node_->getParam("nmea/compat", compat)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/compat must be set"); + if (!node_->get_parameter("nmea.compat", compat)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.compat must be set"); } - if (!node_->getParam("nmea/consider", consider)) { - throw std::runtime_error(std::string("Invalid settings: nmea/set is ") + - "true, therefore nmea/consider must be set"); + if (!node_->get_parameter("nmea.consider", consider)) { + throw std::runtime_error(std::string("Invalid settings: nmea.set is ") + + "true, therefore nmea.consider must be set"); } // set flags cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea/high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea/filter/track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea/gnssToFilter/beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; - getRosUint(node_, "nmea/main_talker_id", cfg_nmea_.main_talker_id); - getRosUint(node_, "nmea/gsv_talker_id", cfg_nmea_.gsv_talker_id); + getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); + getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); std::vector bds_talker_id; - getRosUint(node_, "nmea/bds_talker_id", bds_talker_id); + getRosUint(node_, "nmea.bds_talker_id", bds_talker_id); cfg_nmea_.bds_talker_id[0] = bds_talker_id[0]; cfg_nmea_.bds_talker_id[1] = bds_talker_id[1]; } @@ -133,7 +133,7 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { if (clear_bbr_) { // clear flash memory if (!gps->clearBbr()) { - ROS_ERROR("u-blox failed to clear flash memory"); + RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory"); } } @@ -143,43 +143,43 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // Configure the GNSS, only if the configuration is different // // First, get the current GNSS configuration - ublox_msgs::CfgGNSS cfg_gnss; + ublox_msgs::msg::CfgGNSS cfg_gnss; if (gps->poll(cfg_gnss)) { - ROS_DEBUG("Read GNSS config."); - ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); - ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); } else { throw std::runtime_error("Failed to read the GNSS config."); } // Then, check the configuration for each GNSS. If it is different, change it. bool correct = true; - for (int i = 0; i < cfg_gnss.blocks.size(); i++) { - ublox_msgs::CfgGNSSBlock block = cfg_gnss.blocks[i]; + for (size_t i = 0; i < cfg_gnss.blocks.size(); i++) { + ublox_msgs::msg::CfgGNSSBlock block = cfg_gnss.blocks[i]; if (block.gnss_id == block.GNSS_ID_GPS && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; - ROS_DEBUG("GPS Configuration is different"); + RCLCPP_DEBUG(node_->get_logger(), "GPS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_SBAS - && getRosBoolean(node_, "gnss/sbas") != (block.flags & block.FLAGS_ENABLE)) { + && getRosBoolean(node_, "gnss.sbas") != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss/sbas"); - ROS_DEBUG("SBAS Configuration is different"); + (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss.sbas"); + RCLCPP_DEBUG(node_->get_logger(), "SBAS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_GALILEO && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; - ROS_DEBUG("Galileo GNSS Configuration is different"); + RCLCPP_DEBUG(node_->get_logger(), "Galileo GNSS Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_BEIDOU && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; - ROS_DEBUG("BeiDou Configuration is different"); + RCLCPP_DEBUG(node_->get_logger(), "BeiDou Configuration is different"); } else if (block.gnss_id == block.GNSS_ID_IMES && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { correct = false; @@ -189,14 +189,14 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) || (enable_qzss_ && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { - ROS_DEBUG("QZSS Configuration is different %u, %u", + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration is different %u, %u", block.flags & block.FLAGS_ENABLE, enable_qzss_); correct = false; - ROS_DEBUG("QZSS Configuration: %u", block.flags); + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", block.flags); cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; - ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags); + RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", cfg_gnss.blocks[i].flags); if (enable_qzss_) { // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; @@ -206,14 +206,14 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { correct = false; cfg_gnss.blocks[i].flags = (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; - ROS_DEBUG("GLONASS Configuration is different"); + RCLCPP_DEBUG(node_->get_logger(), "GLONASS Configuration is different"); } } // If the GNSS is already configured correctly, do not re-configure GNSS // since this requires a cold reset if (correct) { - ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured."); + RCLCPP_DEBUG(node_->get_logger(), "U-Blox GNSS configuration is correct. GNSS not re-configured."); } else if (!gps->configGnss(cfg_gnss, std::chrono::seconds(15))) { throw std::runtime_error(std::string("Failed to cold reset device ") + "after configuring GNSS"); @@ -222,7 +222,7 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // // NMEA config // - if (getRosBoolean(node_, "nmea/set") && !gps->configure(cfg_nmea_)) { + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) { throw std::runtime_error("Failed to configure NMEA"); } @@ -231,24 +231,24 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { void UbloxFirmware8::subscribe(std::shared_ptr gps) { // Subscribe to Nav PVT - gps->subscribe( + gps->subscribe( std::bind(&UbloxFirmware7Plus::callbackNavPvt, this, std::placeholders::_1), 1); // Subscribe to Nav SAT messages - if (getRosBoolean(node_, "publish/nav/sat")) { - gps->subscribe([this](const ublox_msgs::NavSAT &m) { nav_sat_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.nav.sat")) { + gps->subscribe([this](const ublox_msgs::msg::NavSAT &m) { nav_sat_pub_->publish(m); }, kNavSvInfoSubscribeRate); } // Subscribe to Mon HW - if (getRosBoolean(node_, "publish/mon/hw")) { - gps->subscribe([this](const ublox_msgs::MonHW &m) { mon_hw_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.mon.hw")) { + gps->subscribe([this](const ublox_msgs::msg::MonHW &m) { mon_hw_pub_->publish(m); }, 1); } // Subscribe to RTCM messages - if (getRosBoolean(node_, "publish/rxm/rtcm")) { - gps->subscribe([this](const ublox_msgs::RxmRTCM &m) { rxm_rtcm_pub_.publish(m); }, + if (getRosBoolean(node_, "publish.rxm.rtcm")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRTCM &m) { rxm_rtcm_pub_->publish(m); }, 1); } } diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp index fddc9e24..320fe357 100644 --- a/ublox_gps/src/ublox_firmware9.cpp +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -1,8 +1,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -11,7 +11,7 @@ namespace ublox_node { -UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, ros::NodeHandle* node) +UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware8(frame_id, updater, freq_diag, gnss, node) { } From 7337ad6d5f2a6d5c308d8ce70540733f6f5d62cb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 2 Dec 2019 23:36:29 +0000 Subject: [PATCH 085/152] Port the ublox package to ROS 2. Signed-off-by: Chris Lalancette --- ublox/CMakeLists.txt | 6 +++--- ublox/COLCON_IGNORE | 0 ublox/package.xml | 12 +++++++----- 3 files changed, 10 insertions(+), 8 deletions(-) delete mode 100644 ublox/COLCON_IGNORE diff --git a/ublox/CMakeLists.txt b/ublox/CMakeLists.txt index c3a0bbb1..a8d8404b 100644 --- a/ublox/CMakeLists.txt +++ b/ublox/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ublox) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/ublox/COLCON_IGNORE b/ublox/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/ublox/package.xml b/ublox/package.xml index d9ae9cd4..20b2e2c9 100644 --- a/ublox/package.xml +++ b/ublox/package.xml @@ -1,5 +1,5 @@ - + ublox 1.2.0 Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. @@ -7,13 +7,15 @@ Veronica Lane BSD - catkin http://wiki.ros.org/ublox + ament_cmake + ublox_serialization + ublox_msgs + ublox_gps + - - - + ament_cmake From b3b065cd554e921c24045b075deb544e531b5368 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 13:35:48 +0000 Subject: [PATCH 086/152] Update configuration files for ROS 2. Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_rover.yaml | 100 +++++++++++++++------------- 1 file changed, 55 insertions(+), 45 deletions(-) diff --git a/ublox_gps/config/c94_m8p_rover.yaml b/ublox_gps/config/c94_m8p_rover.yaml index d16b5747..20fa5b2f 100644 --- a/ublox_gps/config/c94_m8p_rover.yaml +++ b/ublox_gps/config/c94_m8p_rover.yaml @@ -1,46 +1,56 @@ # Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM1 -frame_id: gps -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: auto -enable_ppp: false # Not supported by C94-M8P -dr_limit: 0 - -uart1: - baudrate: 19200 # C94-M8P specific - in: 32 # RTCM 3 - out: 0 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - - nav: - posecef: false +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM0 + frame_id: gps + rate: 4.0 # in Hz + nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: auto + enable_ppp: false # Not supported by C94-M8P + dr_limit: 0 + + # TMODE3 Config + tmode3: 1 # Survey-In Mode + sv_in: + reset: false # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] + + uart1: + baudrate: 19200 # C94-M8P specific + in: 32 # RTCM 3 + out: 0 # No UART out for rover + + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: false # Supported by C94-M8P + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + + nav: + posecef: false From 0aba664d6b75be58b0769f98ae39510888230abc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 13:42:19 +0000 Subject: [PATCH 087/152] Remove declareRosBoolean. It is no longer needed in the ROS 2 port. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 2 +- ublox_gps/include/ublox_gps/utils.hpp | 5 - ublox_gps/src/node.cpp | 158 +++++++++++++------------- 3 files changed, 80 insertions(+), 85 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 57c7394e..59ed5293 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -86,7 +86,7 @@ class UbloxNode final : public rclcpp::Node { constexpr static double kPollDuration = 1.0; // Constants used for diagnostic frequency updater //! [s] 5Hz diagnostic period - constexpr static float kDiagnosticPeriod = 0.2; + float kDiagnosticPeriod = 0.2; //! Tolerance for Fix topic frequency as percentage of target frequency double kFixFreqTol = 0.15; //! Window [num messages] for Fix Frequency Diagnostic diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 8d78f534..1a9bd139 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -206,11 +206,6 @@ bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { return true; } -static inline void declareRosBoolean(rclcpp::Node* node, const std::string &name, bool default_value) -{ - node->declare_parameter(name, default_value); -} - static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) { rclcpp::Parameter parameter; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index fe910c6a..62594f13 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -269,15 +269,15 @@ void UbloxNode::getRosParams() { getRosUint(this, "rtcm.ids", rtcm_ids); // RTCM output message IDs getRosUint(this, "rtcm.rates", rtcm_rates); // RTCM output message rates // PPP: Advanced Setting - declareRosBoolean(this, "enable_ppp", false); + this->declare_parameter("enable_ppp", false); // SBAS params, only for some devices - declareRosBoolean(this, "gnss.sbas", false); - declareRosBoolean(this, "gnss.gps", true); - declareRosBoolean(this, "gnss.glonass", false); - declareRosBoolean(this, "gnss.qzss", false); - declareRosBoolean(this, "gnss.galileo", false); - declareRosBoolean(this, "gnss.beidou", false); - declareRosBoolean(this, "gnss.imes", false); + this->declare_parameter("gnss.sbas", false); + this->declare_parameter("gnss.gps", true); + this->declare_parameter("gnss.glonass", false); + this->declare_parameter("gnss.qzss", false); + this->declare_parameter("gnss.galileo", false); + this->declare_parameter("gnss.beidou", false); + this->declare_parameter("gnss.imes", false); getRosUint(this, "sbas.max", max_sbas_, 0); // Maximum number of SBAS channels getRosUint(this, "sbas.usage", sbas_usage_, 0); dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); @@ -304,7 +304,7 @@ void UbloxNode::getRosParams() { dmodel_ = modelFromString(dynamic_model_); fmode_ = fixModeFromString(fix_mode_); - declareRosBoolean(this, "dat.set", false); + this->declare_parameter("dat.set", false); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) @@ -339,13 +339,13 @@ void UbloxNode::getRosParams() { meas_rate_ = 1000 / rate_; // activate/deactivate any config - declareRosBoolean(this, "config_on_startup", true); - declareRosBoolean(this, "raw_data", false); - declareRosBoolean(this, "clear_bbr", false); - declareRosBoolean(this, "save_on_shutdown", false); - declareRosBoolean(this, "use_adr", true); + this->declare_parameter("config_on_startup", true); + this->declare_parameter("raw_data", false); + this->declare_parameter("clear_bbr", false); + this->declare_parameter("save_on_shutdown", false); + this->declare_parameter("use_adr", true); - declareRosBoolean(this, "sv_in.reset", true); + this->declare_parameter("sv_in.reset", true); this->declare_parameter("sv_in.min_dur", 0); this->declare_parameter("sv_in.acc_lim", 0.0); @@ -355,76 +355,76 @@ void UbloxNode::getRosParams() { rawDataStreamPa_.getRosParams(); // NMEA parameters - declareRosBoolean(this, "nmea.set", false); - declareRosBoolean(this, "nmea.compat", false); - declareRosBoolean(this, "nmea.consider", false); - declareRosBoolean(this, "nmea.limit82", false); - declareRosBoolean(this, "nmea.high_prec", false); - declareRosBoolean(this, "nmea.filter.pos", false); - declareRosBoolean(this, "nmea.filter.msk_pos", false); - declareRosBoolean(this, "nmea.filter.time", false); - declareRosBoolean(this, "nmea.filter.date", false); - declareRosBoolean(this, "nmea.filter.sbas", false); - declareRosBoolean(this, "nmea.filter.track", false); - declareRosBoolean(this, "nmea.filter.gps_only", false); - declareRosBoolean(this, "nmea.gnssToFilter.gps", false); - declareRosBoolean(this, "nmea.gnssToFilter.sbas", false); - declareRosBoolean(this, "nmea.gnssToFilter.qzss", false); - declareRosBoolean(this, "nmea.gnssToFilter.glonass", false); - declareRosBoolean(this, "nmea.gnssToFilter.beidou", false); + this->declare_parameter("nmea.set", false); + this->declare_parameter("nmea.compat", false); + this->declare_parameter("nmea.consider", false); + this->declare_parameter("nmea.limit82", false); + this->declare_parameter("nmea.high_prec", false); + this->declare_parameter("nmea.filter.pos", false); + this->declare_parameter("nmea.filter.msk_pos", false); + this->declare_parameter("nmea.filter.time", false); + this->declare_parameter("nmea.filter.date", false); + this->declare_parameter("nmea.filter.sbas", false); + this->declare_parameter("nmea.filter.track", false); + this->declare_parameter("nmea.filter.gps_only", false); + this->declare_parameter("nmea.gnssToFilter.gps", false); + this->declare_parameter("nmea.gnssToFilter.sbas", false); + this->declare_parameter("nmea.gnssToFilter.qzss", false); + this->declare_parameter("nmea.gnssToFilter.glonass", false); + this->declare_parameter("nmea.gnssToFilter.beidou", false); // Publish parameters - declareRosBoolean(this, "publish.all", false); - - declareRosBoolean(this, "publish.nav.all", getRosBoolean(this, "publish.all")); - declareRosBoolean(this, "publish.nav.att", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.clock", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.heading", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.posecef", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.posllh", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.pvt", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.relposned", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.sat", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.sol", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.svin", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.svinfo", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.status", getRosBoolean(this, "publish.nav.all")); - declareRosBoolean(this, "publish.nav.velned", getRosBoolean(this, "publish.nav.all")); - - declareRosBoolean(this, "publish.rxm.all", getRosBoolean(this, "publish.all")); - declareRosBoolean(this, "publish.rxm.almRaw", getRosBoolean(this, "publish.rxm.all")); - declareRosBoolean(this, "publish.rxm.eph", getRosBoolean(this, "publish.rxm.all")); - declareRosBoolean(this, "publish.rxm.rtcm", getRosBoolean(this, "publish.rxm.all")); - declareRosBoolean(this, "publish.rxm.raw", getRosBoolean(this, "publish.rxm.all")); - declareRosBoolean(this, "publish.rxm.sfrb", getRosBoolean(this, "publish.rxm.all")); - - declareRosBoolean(this, "publish.aid.all", getRosBoolean(this, "publish.all")); - declareRosBoolean(this, "publish.aid.alm", getRosBoolean(this, "publish.aid.all")); - declareRosBoolean(this, "publish.aid.eph", getRosBoolean(this, "publish.aid.all")); - declareRosBoolean(this, "publish.aid.hui", getRosBoolean(this, "publish.aid.all")); - - declareRosBoolean(this, "publish.mon.all", getRosBoolean(this, "publish.all")); - declareRosBoolean(this, "publish.mon.hw", getRosBoolean(this, "publish.mon.all")); - - declareRosBoolean(this, "publish.tim.tm2", false); + this->declare_parameter("publish.all", false); + + this->declare_parameter("publish.nav.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.nav.att", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.clock", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.heading", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.posecef", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.posllh", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.pvt", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.relposned", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.sat", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.sol", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.svin", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.svinfo", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.status", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.velned", getRosBoolean(this, "publish.nav.all")); + + this->declare_parameter("publish.rxm.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.rxm.almRaw", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.eph", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.rtcm", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.raw", getRosBoolean(this, "publish.rxm.all")); + this->declare_parameter("publish.rxm.sfrb", getRosBoolean(this, "publish.rxm.all")); + + this->declare_parameter("publish.aid.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.aid.alm", getRosBoolean(this, "publish.aid.all")); + this->declare_parameter("publish.aid.eph", getRosBoolean(this, "publish.aid.all")); + this->declare_parameter("publish.aid.hui", getRosBoolean(this, "publish.aid.all")); + + this->declare_parameter("publish.mon.all", getRosBoolean(this, "publish.all")); + this->declare_parameter("publish.mon.hw", getRosBoolean(this, "publish.mon.all")); + + this->declare_parameter("publish.tim.tm2", false); // INF parameters - declareRosBoolean(this, "inf.all", true); - declareRosBoolean(this, "inf.debug", false); - declareRosBoolean(this, "inf.error", getRosBoolean(this, "inf.all")); - declareRosBoolean(this, "inf.notice", getRosBoolean(this, "inf.all")); - declareRosBoolean(this, "inf.test", getRosBoolean(this, "inf.all")); - declareRosBoolean(this, "inf.warning", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.all", true); + this->declare_parameter("inf.debug", false); + this->declare_parameter("inf.error", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.notice", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.test", getRosBoolean(this, "inf.all")); + this->declare_parameter("inf.warning", getRosBoolean(this, "inf.all")); // ESF parameters - declareRosBoolean(this, "publish.esf.all", true); - declareRosBoolean(this, "publish.esf.ins", getRosBoolean(this, "publish.esf.all")); - declareRosBoolean(this, "publish.esf.meas", getRosBoolean(this, "publish.esf.all")); - declareRosBoolean(this, "publish.esf.raw", getRosBoolean(this, "publish.esf.all")); - declareRosBoolean(this, "publish.esf.status", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.all", true); + this->declare_parameter("publish.esf.ins", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.meas", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.raw", getRosBoolean(this, "publish.esf.all")); + this->declare_parameter("publish.esf.status", getRosBoolean(this, "publish.esf.all")); // HNR parameters - declareRosBoolean(this, "publish.hnr.pvt", true); + this->declare_parameter("publish.hnr.pvt", true); this->declare_parameter("tmode3", ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED); std::vector empty_double; @@ -433,6 +433,8 @@ void UbloxNode::getRosParams() { this->declare_parameter("arp.position_hp", empty_int); this->declare_parameter("arp.acc", 0.0); this->declare_parameter("arp.lla_flag", false); + + this->declare_parameter("diagnostic_period", kDiagnosticPeriod); } void UbloxNode::pollMessages() { @@ -543,8 +545,6 @@ void UbloxNode::subscribe() { } void UbloxNode::initializeRosDiagnostics() { - declareRosBoolean(this, "diagnostic_period", kDiagnosticPeriod); - for (size_t i = 0; i < components_.size(); i++) { components_[i]->initializeRosDiagnostics(); } From 4ee0a2848e4fc56ece2a71049be796877991a54d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 13:45:43 +0000 Subject: [PATCH 088/152] Mark constants as such. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 2 +- ublox_gps/include/ublox_gps/node.hpp | 8 ++++---- ublox_gps/include/ublox_gps/raw_data_product.hpp | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 5743464d..61c17c51 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -64,7 +64,7 @@ constexpr static unsigned int kBaudrates[] = { 4800, class Gps final { public: //! Sleep time [ms] after setting the baudrate - int kSetBaudrateSleepMs = 500; + const int kSetBaudrateSleepMs = 500; //! Default timeout for ACK messages in seconds constexpr static double kDefaultAckTimeout = 1.0; //! Size of write buffer for output messages diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 59ed5293..613cae28 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -86,13 +86,13 @@ class UbloxNode final : public rclcpp::Node { constexpr static double kPollDuration = 1.0; // Constants used for diagnostic frequency updater //! [s] 5Hz diagnostic period - float kDiagnosticPeriod = 0.2; + const float kDiagnosticPeriod = 0.2; //! Tolerance for Fix topic frequency as percentage of target frequency - double kFixFreqTol = 0.15; + const double kFixFreqTol = 0.15; //! Window [num messages] for Fix Frequency Diagnostic - double kFixFreqWindow = 10; + const double kFixFreqWindow = 10; //! Minimum Time Stamp Status for fix frequency diagnostic - double kTimeStampStatusMin = 0; + const double kTimeStampStatusMin = 0; /** * @brief Initialize and run the u-blox node. diff --git a/ublox_gps/include/ublox_gps/raw_data_product.hpp b/ublox_gps/include/ublox_gps/raw_data_product.hpp index 366bde39..46afef81 100644 --- a/ublox_gps/include/ublox_gps/raw_data_product.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_product.hpp @@ -23,8 +23,8 @@ namespace ublox_node { */ class RawDataProduct final : public virtual ComponentInterface { public: - double kRtcmFreqTol = 0.15; - int kRtcmFreqWindow = 25; + const double kRtcmFreqTol = 0.15; + const int kRtcmFreqWindow = 25; explicit RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node); From 9bc9eb0fa0f4da40e891fed9c69d7f3f1e6462f0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 14:12:42 +0000 Subject: [PATCH 089/152] More fixes around the codebase. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/utils.hpp | 77 +++------------------------ ublox_gps/src/hpg_ref_product.cpp | 28 ++++++++-- ublox_gps/src/node.cpp | 8 +-- 3 files changed, 37 insertions(+), 76 deletions(-) diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 1a9bd139..34bd8883 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -21,7 +21,7 @@ namespace ublox_node { template long toUtcSeconds(const NavPVT& msg) { // Create TM struct for mkgmtime - struct tm time = {0}; + struct tm time{}; time.tm_year = msg.year - 1900; time.tm_mon = msg.month - 1; time.tm_mday = msg.day; @@ -98,13 +98,13 @@ bool getRosUint(rclcpp::Node* node, const std::string& key, U &u) { if (!node->get_parameter(key, parameter)) { return false; } - int param = parameter.get_value(); + U param = parameter.get_value(); // Check the bounds U min = std::numeric_limits::lowest(); U max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output - u = (U) param; + u = static_cast(param); return true; } @@ -145,76 +145,15 @@ bool getRosUint(rclcpp::Node* node, const std::string& key, std::vector &u) { return true; } -/** - * @brief Get a integer (size 8 or 16) value from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -bool getRosInt(rclcpp::Node* node, const std::string& key, I &u) { - rclcpp::Parameter parameter; - if (!node->get_parameter(key, parameter)) { - return false; - } - - int param = parameter.get_value(); - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - // set the output - u = (I) param; - return true; -} - -/** - * @brief Get an integer value (size 8 or 16) from the parameter server. - * @param key the key to be used in the parameter server's dictionary - * @param u storage for the retrieved value. - * @param val value to use if the server doesn't contain this parameter. - * @throws std::runtime_error if the parameter is out of bounds - * @return true if found, false if not found. - */ -template -void getRosInt(rclcpp::Node* node, const std::string& key, U &u, V default_val) { - if (!getRosInt(node, key, u)) { - u = default_val; - } -} - -/** - * @brief Get a int (size 8 or 16) vector from the parameter server. - * @throws std::runtime_error if the parameter is out of bounds. - * @return true if found, false if not found. - */ -template -bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { - std::vector param; - if (!node->get_parameter(key, param)) { - return false; - } - - // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); - checkRange(param, min, max, key); - - // set the output - i.insert(i.begin(), param.begin(), param.end()); - return true; -} - static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) { - rclcpp::Parameter parameter; - if (!node->get_parameter(name, parameter)) { - // Note that if this is used after declareRosBoolean, this should never happen. - throw std::runtime_error("Required parameter '" + name + "' has the wrong type (expected bool)"); + bool ret; + if (!node->get_parameter(name, ret)) { + // Note that if this is used after declare_parameter, this should never happen. + throw std::runtime_error("Required parameter '" + name + "' has not been declared"); } - return parameter.get_value(); + return ret; } } // namespace ublox_node diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index a721b0a4..c971a3da 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -28,6 +28,28 @@ HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ node_->create_publisher("navsvin", 1); } +/** + * @brief Get a int (size 8 or 16) vector from the parameter server. + * @throws std::runtime_error if the parameter is out of bounds. + * @return true if found, false if not found. + */ +template +bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { + std::vector param; + if (!node->get_parameter(key, param)) { + return false; + } + + // Check the bounds + I min = std::numeric_limits::lowest(); + I max = std::numeric_limits::max(); + checkRange(param, min, max, key); + + // set the output + i.insert(i.begin(), param.begin(), param.end()); + return true; +} + void HpgRefProduct::getRosParams() { if (getRosBoolean(node_, "config_on_startup")) { if (nav_rate_ * meas_rate_ != 1000) { @@ -40,15 +62,15 @@ void HpgRefProduct::getRosParams() { if (tmode3_ == ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED) { if (!node_->get_parameter("arp.position", arp_position_)) { - throw std::runtime_error(std::string("Invalid settings: arp/position ") + throw std::runtime_error(std::string("Invalid settings: arp.position ") + "must be set if TMODE3 is fixed"); } if (!getRosInt(node_, "arp.position_hp", arp_position_hp_)) { - throw std::runtime_error(std::string("Invalid settings: arp/position_hp ") + throw std::runtime_error(std::string("Invalid settings: arp.position_hp ") + "must be set if TMODE3 is fixed"); } if (!node_->get_parameter("arp.acc", fixed_pos_acc_)) { - throw std::runtime_error(std::string("Invalid settings: arp/acc ") + throw std::runtime_error(std::string("Invalid settings: arp.acc ") + "must be set if TMODE3 is fixed"); } if (!node_->get_parameter("arp.lla_flag", lla_flag_)) { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 62594f13..b516096f 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -457,13 +457,13 @@ void UbloxNode::pollMessages() { void UbloxNode::printInf(const ublox_msgs::msg::Inf &m, uint8_t id) { if (id == ublox_msgs::Message::INF::ERROR) { - // RCLCPP_ERROR_STREAM(this->get_logger(), "INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); + RCLCPP_ERROR(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); } else if (id == ublox_msgs::Message::INF::WARNING) { - // RCLCPP_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); + RCLCPP_WARN(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); } else if (id == ublox_msgs::Message::INF::DEBUG) { - // RCLCPP_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); + RCLCPP_DEBUG(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); } else { - // RCLCPP_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end(this->get_logger(), ))); + RCLCPP_INFO(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); } } From 519a6b09daca275dbc05c561fb72fe983d134d0b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 14:36:47 +0000 Subject: [PATCH 090/152] More fixes so that we get the correct rates. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/utils.hpp | 15 +++++++++++++++ ublox_gps/src/node.cpp | 22 ++++++++++++---------- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 34bd8883..9a5fc0bd 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -9,6 +9,7 @@ #include +#include #include #include @@ -156,6 +157,20 @@ static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) return ret; } +template +T declareRosIntParameter(rclcpp::Node* node, const std::string& name, long int default_value) +{ + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.name = name; + param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + param_desc.description = name; + rcl_interfaces::msg::IntegerRange range; + range.from_value = std::numeric_limits::lowest(); + range.to_value = std::numeric_limits::max(); + param_desc.integer_range.push_back(range); + return node->declare_parameter(name, default_value, param_desc); +} + } // namespace ublox_node #endif // UBLOX_GPS_UTILS_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index b516096f..b94947db 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -235,10 +235,10 @@ void UbloxNode::getRosParams() { frame_id_ = this->declare_parameter("frame_id", std::string("gps")); // Save configuration parameters - getRosUint(this, "load.mask", load_.load_mask, 0); - getRosUint(this, "load.device", load_.device_mask, 0); - getRosUint(this, "save.mask", save_.save_mask, 0); - getRosUint(this, "save.device", save_.device_mask, 0); + load_.load_mask = declareRosIntParameter(this, "load.mask", 0); + load_.device_mask = declareRosIntParameter(this, "load.device", 0); + save_.save_mask = declareRosIntParameter(this, "save.mask", 0); + save_.device_mask = declareRosIntParameter(this, "save.device", 0); // UART 1 params getRosUint(this, "uart1.baudrate", baudrate_, 9600); @@ -262,7 +262,7 @@ void UbloxNode::getRosParams() { } // Measurement rate params rate_ = this->declare_parameter("rate", 4.0); // in Hz - getRosUint(this, "nav_rate", nav_rate_, 1); // # of measurement rate cycles + nav_rate_ = this->declare_parameter("nav_rate", 1); // # of measurement rate cycles // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; @@ -556,16 +556,18 @@ void UbloxNode::processMonVer() { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); } - // RCLCPP_DEBUG(this->get_logger(), "%s, HW VER: %s", monVer.sw_version.array(), - // monVer.hw_version.array()); + RCLCPP_INFO(this->get_logger(), "%s, HW VER: %s", + std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(), + std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str()); // Convert extension to vector of strings std::vector extension; extension.reserve(monVer.extension.size()); for (std::size_t i = 0; i < monVer.extension.size(); ++i) { - // RCLCPP_DEBUG(this->get_logger(), "%s", monVer.extension[i].field.array()); + RCLCPP_DEBUG(this->get_logger(), "%s", + std::string(monVer.extension[i].field.begin(), monVer.extension[i].field.end()).c_str()); // Find the end of the string (null character) unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); + monVer.extension[i].field.end(), '\0'); extension.push_back(std::string(monVer.extension[i].field.begin(), end)); } @@ -580,7 +582,7 @@ void UbloxNode::processMonVer() { } if (protocol_version_ == 0) { RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", - "Defaulting to firmware version 6."); + "Defaulting to firmware version 6."); } addFirmwareInterface(); From f6eaea27198e9113a6ef5dae041e2b7736f9a2d1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 14:47:48 +0000 Subject: [PATCH 091/152] Declare more parameters correctly. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index b94947db..4e0e08de 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -225,8 +225,8 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared()); } else if (product_category.compare("SPG") != 0) { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", - product_category.c_str(), ref_rov.c_str(), - "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); + product_category.c_str(), ref_rov.c_str(), + "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); } } @@ -241,11 +241,11 @@ void UbloxNode::getRosParams() { save_.device_mask = declareRosIntParameter(this, "save.device", 0); // UART 1 params - getRosUint(this, "uart1.baudrate", baudrate_, 9600); - getRosUint(this, "uart1.in", uart_in_, ublox_msgs::msg::CfgPRT::PROTO_UBX - | ublox_msgs::msg::CfgPRT::PROTO_NMEA - | ublox_msgs::msg::CfgPRT::PROTO_RTCM); - getRosUint(this, "uart1.out", uart_out_, ublox_msgs::msg::CfgPRT::PROTO_UBX); + baudrate_ = declareRosIntParameter(this, "uart1.baudrate", 9600); + uart_in_ = declareRosIntParameter(this, "uart1.in", ublox_msgs::msg::CfgPRT::PROTO_UBX + | ublox_msgs::msg::CfgPRT::PROTO_NMEA + | ublox_msgs::msg::CfgPRT::PROTO_RTCM); + uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; if (this->has_parameter("usb.in") || this->has_parameter("usb.out")) { @@ -262,7 +262,7 @@ void UbloxNode::getRosParams() { } // Measurement rate params rate_ = this->declare_parameter("rate", 4.0); // in Hz - nav_rate_ = this->declare_parameter("nav_rate", 1); // # of measurement rate cycles + nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; @@ -278,11 +278,11 @@ void UbloxNode::getRosParams() { this->declare_parameter("gnss.galileo", false); this->declare_parameter("gnss.beidou", false); this->declare_parameter("gnss.imes", false); - getRosUint(this, "sbas.max", max_sbas_, 0); // Maximum number of SBAS channels - getRosUint(this, "sbas.usage", sbas_usage_, 0); + max_sbas_ = declareRosIntParameter(this, "sbas.max", 0); // Maximum number of SBAS channels + sbas_usage_ = declareRosIntParameter(this, "sbas.usage", 0); dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); fix_mode_ = this->declare_parameter("fix_mode", std::string("auto")); - getRosUint(this, "dr_limit", dr_limit_, 0); // Dead reckoning limit + dr_limit_ = declareRosIntParameter(this, "dr_limit", 0); // Dead reckoning limit if (getRosBoolean(this, "enable_ppp")) { RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); From c4f3b34e9d858a370888f575f375fc49736bfe1a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 15:37:52 +0000 Subject: [PATCH 092/152] Declare more parameters. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/utils.hpp | 7 +++++++ ublox_gps/src/node.cpp | 26 ++++++++++++++------------ 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 9a5fc0bd..ff4826a4 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -171,6 +171,13 @@ T declareRosIntParameter(rclcpp::Node* node, const std::string& name, long int d return node->declare_parameter(name, default_value, param_desc); } +static inline bool isRosParameterSet(rclcpp::Node* node, const std::string& name) +{ + rclcpp::Parameter param; + node->get_parameter(name, param); + return param.get_type() != rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} + } // namespace ublox_node #endif // UBLOX_GPS_UTILS_HPP diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 4e0e08de..f2213aa0 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -248,21 +248,26 @@ void UbloxNode::getRosParams() { uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - if (this->has_parameter("usb.in") || this->has_parameter("usb.out")) { + this->declare_parameter("usb.in"); + this->declare_parameter("usb.out"); + usb_tx_ = declareRosIntParameter(this, "usb.tx_ready", 0); + if (isRosParameterSet(this, "usb.in") || isRosParameterSet(this, "usb.out")) { set_usb_ = true; if (!getRosUint(this, "usb.in", usb_in_)) { throw std::runtime_error(std::string("usb.out is set, therefore ") + - "usb.in must be set"); + "usb.in must be set"); } if (!getRosUint(this, "usb.out", usb_out_)) { throw std::runtime_error(std::string("usb.in is set, therefore ") + - "usb.out must be set"); + "usb.out must be set"); } - getRosUint(this, "usb.tx_ready", usb_tx_, 0); } // Measurement rate params rate_ = this->declare_parameter("rate", 4.0); // in Hz + checkMin(rate_, 0.0, "rate"); + nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles + // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; @@ -288,8 +293,6 @@ void UbloxNode::getRosParams() { RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); } - checkMin(rate_, 0, "rate"); - if (rtcm_ids.size() != rtcm_rates.size()) { throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); @@ -305,6 +308,7 @@ void UbloxNode::getRosParams() { fmode_ = fixModeFromString(fix_mode_); this->declare_parameter("dat.set", false); + this->declare_parameter("dat.majA"); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) @@ -349,7 +353,7 @@ void UbloxNode::getRosParams() { this->declare_parameter("sv_in.min_dur", 0); this->declare_parameter("sv_in.acc_lim", 0.0); - this->declare_parameter("dgnss_mode", ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FLOAT); + this->declare_parameter("dgnss_mode"); // raw data stream logging rawDataStreamPa_.getRosParams(); @@ -426,11 +430,9 @@ void UbloxNode::getRosParams() { // HNR parameters this->declare_parameter("publish.hnr.pvt", true); - this->declare_parameter("tmode3", ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED); - std::vector empty_double; - std::vector empty_int; - this->declare_parameter("arp.position", empty_double); - this->declare_parameter("arp.position_hp", empty_int); + this->declare_parameter("tmode3"); + this->declare_parameter("arp.position"); + this->declare_parameter("arp.position_hp"); this->declare_parameter("arp.acc", 0.0); this->declare_parameter("arp.lla_flag", false); From e49e84b5f298d9c6c2c51a2ba4e8c41df5d9b7ca Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 15:50:42 +0000 Subject: [PATCH 093/152] More fixes for declaring parameters. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 38 +++++++++++++++------------ ublox_gps/package.xml | 1 + ublox_gps/src/node.cpp | 55 ++++++++++++++++++++++------------------ 3 files changed, 53 insertions(+), 41 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 5e5b54a0..d5e6c3e8 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(asio REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -25,26 +26,37 @@ find_package(ublox_serialization REQUIRED) include_directories(include) -# build library -add_library(ublox_gps src/gps.cpp) -ament_target_dependencies(ublox_gps - "asio" - "ublox_msgs" - "ublox_serialization" -) - # build node -add_executable(ublox_gps_node src/node.cpp src/mkgmtime.c src/raw_data_pa.cpp src/gnss.cpp src/tim_product.cpp src/adr_udr_product.cpp src/hpg_rov_product.cpp src/raw_data_product.cpp src/ublox_firmware.cpp src/ublox_firmware6.cpp src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp src/hpg_ref_product.cpp src/hp_pos_rec_product.cpp src/node_main.cpp) +add_executable(ublox_gps_node + src/adr_udr_product.cpp + src/gnss.cpp + src/gps.cpp + src/hp_pos_rec_product.cpp + src/hpg_ref_product.cpp + src/hpg_rov_product.cpp + src/mkgmtime.c + src/node.cpp + src/node_main.cpp + src/raw_data_pa.cpp + src/raw_data_product.cpp + src/tim_product.cpp + src/ublox_firmware.cpp + src/ublox_firmware6.cpp + src/ublox_firmware7.cpp + src/ublox_firmware8.cpp + src/ublox_firmware9.cpp) ament_target_dependencies(ublox_gps_node + "asio" "diagnostic_msgs" "diagnostic_updater" "geometry_msgs" + "rcl_interfaces" "rclcpp" "sensor_msgs" "std_msgs" "ublox_msgs" + "ublox_serialization" ) -target_link_libraries(ublox_gps_node ublox_gps) # build logger node add_executable(ublox_logger_node src/logger_node_pa.cpp src/raw_data_pa.cpp) @@ -54,12 +66,6 @@ ament_target_dependencies(ublox_logger_node "std_msgs" ) -install(TARGETS ublox_gps - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - install(TARGETS ublox_gps_node ublox_logger_node DESTINATION lib/${PROJECT_NAME} diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 6c2751f2..62dcc428 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -18,6 +18,7 @@ diagnostic_msgs diagnostic_updater geometry_msgs + rcl_interfaces rclcpp sensor_msgs std_msgs diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f2213aa0..d575241a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -161,10 +161,9 @@ std::vector stringSplit(const std::string &str, UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { int debug = this->declare_parameter("debug", 1); if (debug) { - // if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - // ros::console::levels::Debug)) { - // ros::console::notifyLoggerLevelsChanged(); - // } + if (rcutils_logging_set_logger_level("ublox_gps_node", RCUTILS_LOG_SEVERITY_DEBUG) != RCUTILS_RET_OK) { + RCLCPP_WARN(this->get_logger(), "Failed to set the debugging level"); + } } gps_ = std::make_shared(debug); @@ -271,10 +270,27 @@ void UbloxNode::getRosParams() { // RTCM params std::vector rtcm_ids; std::vector rtcm_rates; + this->declare_parameter("rtcm.ids"); + this->declare_parameter("rtcm.rates"); getRosUint(this, "rtcm.ids", rtcm_ids); // RTCM output message IDs getRosUint(this, "rtcm.rates", rtcm_rates); // RTCM output message rates + if (rtcm_ids.size() != rtcm_rates.size()) { + throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + + " must match size of rtcm_rates"); + } + + rtcms_.resize(rtcm_ids.size()); + for (size_t i = 0; i < rtcm_ids.size(); ++i) { + rtcms_[i].id = rtcm_ids[i]; + rtcms_[i].rate = rtcm_rates[i]; + } + // PPP: Advanced Setting this->declare_parameter("enable_ppp", false); + if (getRosBoolean(this, "enable_ppp")) { + RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); + } + // SBAS params, only for some devices this->declare_parameter("gnss.sbas", false); this->declare_parameter("gnss.gps", true); @@ -286,36 +302,25 @@ void UbloxNode::getRosParams() { max_sbas_ = declareRosIntParameter(this, "sbas.max", 0); // Maximum number of SBAS channels sbas_usage_ = declareRosIntParameter(this, "sbas.usage", 0); dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); + dmodel_ = modelFromString(dynamic_model_); fix_mode_ = this->declare_parameter("fix_mode", std::string("auto")); + fmode_ = fixModeFromString(fix_mode_); dr_limit_ = declareRosIntParameter(this, "dr_limit", 0); // Dead reckoning limit - if (getRosBoolean(this, "enable_ppp")) { - RCLCPP_WARN(this->get_logger(), "Warning: PPP is enabled - this is an expert setting."); - } - - if (rtcm_ids.size() != rtcm_rates.size()) { - throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + - " must match size of rtcm_rates"); - } - - rtcms_.resize(rtcm_ids.size()); - for (size_t i = 0; i < rtcm_ids.size(); ++i) { - rtcms_[i].id = rtcm_ids[i]; - rtcms_[i].rate = rtcm_rates[i]; - } - - dmodel_ = modelFromString(dynamic_model_); - fmode_ = fixModeFromString(fix_mode_); this->declare_parameter("dat.set", false); this->declare_parameter("dat.majA"); + this->declare_parameter("dat.flat"); + this->declare_parameter("dat.shift"); + this->declare_parameter("dat.rot"); + this->declare_parameter("dat.scale"); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) - || this->get_parameter("dat.flat", cfg_dat_.flat) - || this->get_parameter("dat.shift", shift) - || this->get_parameter("dat.rot", rot) - || this->get_parameter("dat.scale", cfg_dat_.scale)) { + || !this->get_parameter("dat.flat", cfg_dat_.flat) + || !this->get_parameter("dat.shift", shift) + || !this->get_parameter("dat.rot", rot) + || !this->get_parameter("dat.scale", cfg_dat_.scale)) { throw std::runtime_error(std::string("dat.set is true, therefore ") + "dat.majA, dat.flat, dat.shift, dat.rot, & dat.scale must be set"); } From f0113b15a1bc68cb8e94bd3877d5b4bb8a82e817 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 16:03:36 +0000 Subject: [PATCH 094/152] Minor cleanup in the gps code. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/callback.hpp | 2 +- ublox_gps/src/gps.cpp | 34 +++++++++++++----------- ublox_gps/src/node.cpp | 2 +- 3 files changed, 21 insertions(+), 17 deletions(-) diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 119ab0c1..fe49c5de 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -100,7 +100,7 @@ class CallbackHandler_ final : public CallbackHandler { condition_.notify_all(); return; } - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { // RCLCPP_DEBUG_COND(debug_ >= 2, // "U-Blox Decoder error for 0x%02x / 0x%02x (%d bytes)", // static_cast(reader.classId()), diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 18afd306..9e5b7358 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -51,7 +51,9 @@ Gps::Gps(int debug) : configured_(false), config_on_startup_flag_(true), debug_( subscribeAcks(); } -Gps::~Gps() { close(); } +Gps::~Gps() { + close(); +} void Gps::setWorker(const std::shared_ptr& worker) { if (worker_) { @@ -59,20 +61,23 @@ void Gps::setWorker(const std::shared_ptr& worker) { } worker_ = worker; worker_->setCallback(std::bind(&CallbackHandlers::readCallback, - &callbacks_, std::placeholders::_1, std::placeholders::_2)); + &callbacks_, std::placeholders::_1, + std::placeholders::_2)); configured_ = static_cast(worker); } void Gps::subscribeAcks() { // Set NACK handler - subscribeId(std::bind(&Gps::processNack, this, std::placeholders::_1), - ublox_msgs::Message::ACK::NACK); + subscribeId(std::bind(&Gps::processNack, this, + std::placeholders::_1), + ublox_msgs::Message::ACK::NACK); // Set ACK handler - subscribeId(std::bind(&Gps::processAck, this, std::placeholders::_1), - ublox_msgs::Message::ACK::ACK); + subscribeId(std::bind(&Gps::processAck, this, + std::placeholders::_1), + ublox_msgs::Message::ACK::ACK); // Set UPD-SOS-ACK handler - subscribe( - std::bind(&Gps::processUpdSosAck, this, std::placeholders::_1)); + subscribe(std::bind(&Gps::processUpdSosAck, this, + std::placeholders::_1)); } void Gps::processAck(const ublox_msgs::msg::Ack &m) { @@ -123,7 +128,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } @@ -148,7 +153,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (size_t i = 0; i < sizeof(kBaudrates)/sizeof(kBaudrates[0]); i++) { + for (size_t i = 0; i < sizeof(kBaudrates) / sizeof(kBaudrates[0]); i++) { if (current_baudrate.value() == baudrate) { break; } @@ -156,8 +161,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) { continue; } - serial->set_option( - asio::serial_port_base::baud_rate(kBaudrates[i])); + serial->set_option(asio::serial_port_base::baud_rate(kBaudrates[i])); std::this_thread::sleep_for( std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); @@ -180,7 +184,7 @@ void Gps::resetSerial(const std::string & port) { // open serial port try { serial->open(port); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not open serial port :" + port + " " + e.what()); } @@ -223,7 +227,7 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { asio::ip::tcp::resolver resolver(*io_service); endpoint = resolver.resolve(asio::ip::tcp::resolver::query(host, port)); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not resolve" + host + " " + port + " " + e.what()); } @@ -232,7 +236,7 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { try { socket->connect(*endpoint); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { throw std::runtime_error("U-Blox: Could not connect to " + endpoint->host_name() + ":" + endpoint->service_name() + ": " + e.what()); diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index d575241a..da85a7ae 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -703,7 +703,7 @@ bool UbloxNode::configureUblox() { RCLCPP_ERROR(this->get_logger(), "u-blox unable to save configuration to non-volatile memory"); } } - } catch (std::exception& e) { + } catch (const std::exception& e) { RCLCPP_FATAL(this->get_logger(), "Error configuring u-blox: %s", e.what()); return false; } From a199564b575d5984fcd86090af6ee9d34368482f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 16:15:33 +0000 Subject: [PATCH 095/152] Pass the logger down to the Gps level. That way we can get more debugging. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/gps.hpp | 11 ++- ublox_gps/src/gps.cpp | 102 ++++++++++++++-------------- ublox_gps/src/node.cpp | 2 +- 3 files changed, 61 insertions(+), 54 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 61c17c51..228fe70a 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -36,6 +36,9 @@ #include #include #include + +#include + // Other u-blox packages #include // u-blox gps @@ -70,7 +73,7 @@ class Gps final { //! Size of write buffer for output messages constexpr static int kWriterSize = 2056; - explicit Gps(int debug); + explicit Gps(int debug, const rclcpp::Logger & logger); ~Gps(); /** @@ -478,6 +481,8 @@ class Gps final { CallbackHandlers callbacks_; std::string host_, port_; + + rclcpp::Logger logger_; }; template @@ -533,8 +538,8 @@ bool Gps::configure(const ConfigT& message, bool wait) { std::vector out(kWriterSize); ublox::Writer writer(out.data(), out.size()); if (!writer.write(message)) { - // RCLCPP_ERROR("Failed to encode config message 0x%02x / 0x%02x", - // message.CLASS_ID, message.MESSAGE_ID); + RCLCPP_ERROR(logger_, "Failed to encode config message 0x%02x / 0x%02x", + message.CLASS_ID, message.MESSAGE_ID); return false; } // Send the message to the device diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 9e5b7358..79779534 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -38,6 +38,8 @@ #include #include +#include + #include #include @@ -47,7 +49,7 @@ const std::chrono::milliseconds Gps::default_timeout_ = std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps(int debug) : configured_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug) { +Gps::Gps(int debug, const rclcpp::Logger& logger) : configured_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug), logger_(logger) { subscribeAcks(); } @@ -88,8 +90,8 @@ void Gps::processAck(const ublox_msgs::msg::Ack &m) { ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - // RCLCPP_DEBUG_COND(debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", - // m.cls_id, m.msg_id); + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "U-blox: received ACK: 0x%02x / 0x%02x", + m.cls_id, m.msg_id); } void Gps::processNack(const ublox_msgs::msg::Ack &m) { @@ -100,7 +102,7 @@ void Gps::processNack(const ublox_msgs::msg::Ack &m) { ack.msg_id = m.msg_id; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - // RCLCPP_ERROR("U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); + RCLCPP_ERROR(logger_, "U-blox: received NACK: 0x%02x / 0x%02x", m.cls_id, m.msg_id); } void Gps::processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m) { @@ -111,10 +113,10 @@ void Gps::processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m) { ack.msg_id = m.MESSAGE_ID; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); - // RCLCPP_DEBUG_COND(ack.type == ACK && debug_ >= 2, - // "U-blox: received UPD SOS Backup ACK"); + RCLCPP_DEBUG_EXPRESSION(logger_, ack.type == ACK && debug_ >= 2, + "U-blox: received UPD SOS Backup ACK"); if (ack.type == NACK) { - // RCLCPP_ERROR("U-blox: received UPD SOS Backup NACK"); + RCLCPP_ERROR(logger_, "U-blox: received UPD SOS Backup NACK"); } } } @@ -133,7 +135,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, + port + " " + e.what()); } - // RCLCPP_INFO("U-Blox: Opened serial port %s", port.c_str()); + RCLCPP_INFO(logger_, "U-Blox: Opened serial port %s", port.c_str()); int fd = serial->native_handle(); termios tio; @@ -165,7 +167,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, std::this_thread::sleep_for( std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); - // RCLCPP_DEBUG("U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); + RCLCPP_DEBUG(logger_, "U-Blox: Set ASIO baudrate to %u", current_baudrate.value()); } if (config_on_startup_flag_) { configured_ = configUart1(baudrate, uart_in, uart_out); @@ -189,7 +191,7 @@ void Gps::resetSerial(const std::string & port) { + port + " " + e.what()); } - // RCLCPP_INFO("U-Blox: Reset serial port %s", port.c_str()); + RCLCPP_INFO(logger_, "U-Blox: Reset serial port %s", port.c_str()); // Set the I/O worker if (worker_) { @@ -202,13 +204,13 @@ void Gps::resetSerial(const std::string & port) { std::vector payload; payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { - // RCLCPP_ERROR("Resetting Serial Port: Could not poll UART1 CfgPRT"); + RCLCPP_ERROR(logger_, "Resetting Serial Port: Could not poll UART1 CfgPRT"); return; } ublox_msgs::msg::CfgPRT prt; if (!read(prt, default_timeout_)) { - // RCLCPP_ERROR("Resetting Serial Port: Could not read polled UART1 CfgPRT %s", - // "message"); + RCLCPP_ERROR(logger_, "Resetting Serial Port: Could not read polled UART1 CfgPRT %s", + "message"); return; } @@ -242,8 +244,8 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { endpoint->service_name() + ": " + e.what()); } - // RCLCPP_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), - // endpoint->service_name().c_str()); + RCLCPP_INFO(logger_, "U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), + endpoint->service_name().c_str()); if (worker_) { return; @@ -255,9 +257,9 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { void Gps::close() { if (save_on_shutdown_) { if (saveOnShutdown()) { - // RCLCPP_INFO("U-Blox Flash BBR saved"); + RCLCPP_INFO(logger_, "U-Blox Flash BBR saved"); } else { - // RCLCPP_INFO("U-Blox Flash BBR failed to save"); + RCLCPP_INFO(logger_, "U-Blox Flash BBR failed to save"); } } worker_.reset(); @@ -277,8 +279,8 @@ void Gps::reset(const std::chrono::milliseconds& wait) { } bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { - // RCLCPP_WARN("Resetting u-blox. If device address changes, %s", - // "node must be relaunched."); + RCLCPP_WARN(logger_, "Resetting u-blox. If device address changes, %s", + "node must be relaunched."); ublox_msgs::msg::CfgRST rst; rst.nav_bbr_mask = nav_bbr_mask; @@ -294,12 +296,12 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { bool Gps::configGnss(ublox_msgs::msg::CfgGNSS gnss, const std::chrono::milliseconds& wait) { // Configure the GNSS settingshttps://mail.google.com/mail/u/0/#inbox - // RCLCPP_DEBUG("Re-configuring GNSS."); + RCLCPP_DEBUG(logger_, "Re-configuring GNSS."); if (!configure(gnss)) { return false; } // Cold reset the GNSS - // RCLCPP_WARN("GNSS re-configured, cold resetting device."); + RCLCPP_WARN(logger_, "GNSS re-configured, cold resetting device."); if (!configReset(ublox_msgs::msg::CfgRST::NAV_BBR_COLD_START, ublox_msgs::msg::CfgRST::RESET_MODE_GNSS)) { return false; } @@ -337,8 +339,8 @@ bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, return true; } - // RCLCPP_DEBUG("Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", - // baudrate, in_proto_mask, out_proto_mask); + RCLCPP_DEBUG(logger_, "Configuring UART1 baud rate: %u, In/Out Protocol: %u / %u", + baudrate, in_proto_mask, out_proto_mask); ublox_msgs::msg::CfgPRT port; port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_UART1; @@ -351,17 +353,17 @@ bool Gps::configUart1(unsigned int baudrate, uint16_t in_proto_mask, } bool Gps::disableUart1(ublox_msgs::msg::CfgPRT& prev_config) { - // RCLCPP_DEBUG("Disabling UART1"); + RCLCPP_DEBUG(logger_, "Disabling UART1"); // Poll UART PRT Config std::vector payload; payload.push_back(ublox_msgs::msg::CfgPRT::PORT_ID_UART1); if (!poll(ublox_msgs::msg::CfgPRT::CLASS_ID, ublox_msgs::msg::CfgPRT::MESSAGE_ID, payload)) { - // RCLCPP_ERROR("disableUart: Could not poll UART1 CfgPRT"); + RCLCPP_ERROR(logger_, "disableUart: Could not poll UART1 CfgPRT"); return false; } if (!read(prev_config, default_timeout_)) { - // RCLCPP_ERROR("disableUart: Could not read polled UART1 CfgPRT message"); + RCLCPP_ERROR(logger_, "disableUart: Could not read polled UART1 CfgPRT message"); return false; } // Keep original settings, but disable in/out @@ -383,8 +385,8 @@ bool Gps::configUsb(uint16_t tx_ready, return true; } - // RCLCPP_DEBUG("Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", - // tx_ready, in_proto_mask, out_proto_mask); + RCLCPP_DEBUG(logger_, "Configuring USB tx_ready: %u, In/Out Protocol: %u / %u", + tx_ready, in_proto_mask, out_proto_mask); ublox_msgs::msg::CfgPRT port; port.port_id = ublox_msgs::msg::CfgPRT::PORT_ID_USB; @@ -395,8 +397,8 @@ bool Gps::configUsb(uint16_t tx_ready, } bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { - // RCLCPP_DEBUG("Configuring measurement rate to %u ms and nav rate to %u cycles", - // meas_rate, nav_rate); + RCLCPP_DEBUG(logger_, "Configuring measurement rate to %u ms and nav rate to %u cycles", + meas_rate, nav_rate); ublox_msgs::msg::CfgRATE rate; rate.meas_rate = meas_rate; @@ -407,9 +409,9 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { bool Gps::configRtcm(const std::vector & rtcms) { for (size_t i = 0; i < rtcms.size(); ++i) { - // RCLCPP_DEBUG("Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); + RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); if (!setRate(ublox_msgs::Class::RTCM, rtcms[i].id, rtcms[i].rate)) { - // RCLCPP_ERROR("Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); + RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); return false; } } @@ -417,7 +419,7 @@ bool Gps::configRtcm(const std::vector & rtcms) { } bool Gps::configSbas(bool enable, uint8_t usage, uint8_t max_sbas) { - // RCLCPP_DEBUG("Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); + RCLCPP_DEBUG(logger_, "Configuring SBAS: usage %u, max_sbas %u", usage, max_sbas); ublox_msgs::msg::CfgSBAS msg; msg.mode = (enable ? ublox_msgs::msg::CfgSBAS::MODE_ENABLED : 0); @@ -431,12 +433,12 @@ bool Gps::configTmode3Fixed(bool lla_flag, std::vector arp_position_hp, float fixed_pos_acc) { if (arp_position.size() != 3 || arp_position_hp.size() != 3) { - // RCLCPP_ERROR("Configuring TMODE3 to Fixed: size of position %s", - // "& arp_position_hp args must be 3"); + RCLCPP_ERROR(logger_, "Configuring TMODE3 to Fixed: size of position %s", + "& arp_position_hp args must be 3"); return false; } - // RCLCPP_DEBUG("Configuring TMODE3 to Fixed"); + RCLCPP_DEBUG(logger_, "Configuring TMODE3 to Fixed"); ublox_msgs::msg::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK; @@ -465,7 +467,7 @@ bool Gps::configTmode3Fixed(bool lla_flag, bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) { ublox_msgs::msg::CfgTMODE3 tmode3; - // RCLCPP_DEBUG("Setting TMODE3 to Survey In"); + RCLCPP_DEBUG(logger_, "Setting TMODE3 to Survey In"); tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; tmode3.svin_min_dur = svin_min_dur; // Convert from m to [0.1 mm] @@ -474,7 +476,7 @@ bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, } bool Gps::disableTmode3() { - // RCLCPP_DEBUG("Disabling TMODE3"); + RCLCPP_DEBUG(logger_, "Disabling TMODE3"); ublox_msgs::msg::CfgTMODE3 tmode3; tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK; @@ -482,8 +484,8 @@ bool Gps::disableTmode3() { } bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { - // RCLCPP_DEBUG_COND(debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, - // message_id, rate); + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id, + message_id, rate); ublox_msgs::msg::CfgMSG msg; msg.msg_class = class_id; msg.msg_id = message_id; @@ -492,7 +494,7 @@ bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) { } bool Gps::setDynamicModel(uint8_t model) { - // RCLCPP_DEBUG("Setting dynamic model to %u", model); + RCLCPP_DEBUG(logger_, "Setting dynamic model to %u", model); ublox_msgs::msg::CfgNAV5 msg; msg.dyn_model = model; @@ -501,7 +503,7 @@ bool Gps::setDynamicModel(uint8_t model) { } bool Gps::setFixMode(uint8_t mode) { - // RCLCPP_DEBUG("Setting fix mode to %u", mode); + RCLCPP_DEBUG(logger_, "Setting fix mode to %u", mode); ublox_msgs::msg::CfgNAV5 msg; msg.fix_mode = mode; @@ -510,7 +512,7 @@ bool Gps::setFixMode(uint8_t mode) { } bool Gps::setDeadReckonLimit(uint8_t limit) { - // RCLCPP_DEBUG("Setting DR Limit to %u", limit); + RCLCPP_DEBUG(logger_, "Setting DR Limit to %u", limit); ublox_msgs::msg::CfgNAV5 msg; msg.dr_limit = limit; @@ -519,7 +521,7 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { } bool Gps::setPpp(bool enable) { - // RCLCPP_DEBUG("%s PPP", (enable ? "Enabling" : "Disabling")); + RCLCPP_DEBUG(logger_,"%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_ppp = enable; @@ -529,13 +531,13 @@ bool Gps::setPpp(bool enable) { bool Gps::setDgnss(uint8_t mode) { ublox_msgs::msg::CfgDGNSS cfg; - // RCLCPP_DEBUG("Setting DGNSS mode to %u", mode); + RCLCPP_DEBUG(logger_, "Setting DGNSS mode to %u", mode); cfg.dgnss_mode = mode; return configure(cfg); } bool Gps::setUseAdr(bool enable) { - // RCLCPP_DEBUG("%s ADR/UDR", (enable ? "Enabling" : "Disabling")); + RCLCPP_DEBUG(logger_, "%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_adr = enable; @@ -561,8 +563,8 @@ bool Gps::poll(uint8_t class_id, uint8_t message_id, bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, uint8_t class_id, uint8_t msg_id) { - // RCLCPP_DEBUG_COND(debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", - // class_id, msg_id); + RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Waiting for ACK 0x%02x / 0x%02x", + class_id, msg_id); std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); std::chrono::system_clock::time_point wait_until = now + timeout; @@ -588,7 +590,7 @@ void Gps::setRawDataCallback(const Worker::Callback& callback) { } bool Gps::setUTCtime() { - // RCLCPP_DEBUG("Setting time to UTC time"); + RCLCPP_DEBUG(logger_, "Setting time to UTC time"); ublox_msgs::msg::CfgNAV5 msg; msg.utc_standard = 3; @@ -596,7 +598,7 @@ bool Gps::setUTCtime() { } bool Gps::setTimtm2(uint8_t rate) { - // RCLCPP_DEBUG("TIM-TM2 send rate on current port set to %u", rate); + RCLCPP_DEBUG(logger_, "TIM-TM2 send rate on current port set to %u", rate); ublox_msgs::msg::CfgMSG msg; msg.msg_class = ublox_msgs::msg::TimTM2::CLASS_ID; msg.msg_id = ublox_msgs::msg::TimTM2::MESSAGE_ID; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index da85a7ae..8d9b9efd 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -166,7 +166,7 @@ UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { } } - gps_ = std::make_shared(debug); + gps_ = std::make_shared(debug, this->get_logger()); gnss_ = std::make_shared(); From a31f9617442f558d819e09768a7d246af458e1dc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 16:35:53 +0000 Subject: [PATCH 096/152] Fix a few bugs pointed out by valgrind. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- ublox_gps/include/ublox_gps/fix_diagnostic.hpp | 1 + ublox_gps/include/ublox_gps/gps.hpp | 4 +++- ublox_gps/include/ublox_gps/hpg_ref_product.hpp | 2 +- ublox_gps/src/node.cpp | 10 ++++++---- 5 files changed, 12 insertions(+), 7 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index d5e6c3e8..4cd317b9 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -ggdb3) endif() set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") diff --git a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp index f9b2a4af..3c9d7b1d 100644 --- a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp @@ -29,6 +29,7 @@ struct FixDiagnostic { double stamp_min, uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz + //const double target_freq = 1.0; min_freq = target_freq; max_freq = target_freq; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 228fe70a..fca143ec 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -473,7 +473,7 @@ class Gps final { //! The default timeout for ACK messages static const std::chrono::milliseconds default_timeout_; //! Stores last received ACK accessed by multiple threads - mutable std::atomic ack_; + std::atomic ack_{}; int debug_; @@ -532,6 +532,8 @@ bool Gps::configure(const ConfigT& message, bool wait) { // Reset ack Ack ack; ack.type = WAIT; + ack.class_id = 0; + ack.msg_id = 0; ack_.store(ack, std::memory_order_seq_cst); // Encode the message diff --git a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp index 45fc6a79..ff386ef3 100644 --- a/ublox_gps/include/ublox_gps/hpg_ref_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_ref_product.hpp @@ -127,7 +127,7 @@ class HpgRefProduct: public virtual ComponentInterface { DISABLED, //!< Time mode disabled SURVEY_IN, //!< Survey-In mode TIME //!< Time mode, after survey-in or after configuring fixed mode - } mode_; + } mode_{INIT}; rclcpp::Publisher::SharedPtr navsvin_pub_; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 8d9b9efd..e080b41f 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -180,10 +180,6 @@ UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { updater_ = std::make_shared(this); updater_->setHardwareID("ublox"); - // configure diagnostic updater for frequency - freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); - initialize(); } @@ -775,6 +771,12 @@ void UbloxNode::initializeIo() { void UbloxNode::initialize() { // Params must be set before initializing IO getRosParams(); + + // configure diagnostic updater for frequency + freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, + kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); + + initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); From f99692f10e1e3312d94c4f1f4d4245f86784dbcf Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2019 21:43:43 +0000 Subject: [PATCH 097/152] Remove -ggdb3 flag from CMakeLists.txt. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 4cd317b9..d5e6c3e8 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -ggdb3) + add_compile_options(-Wall -Wextra -Wpedantic) endif() set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") From 1451923f18e169140cef3c3a81abc47378588e90 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Dec 2019 11:18:28 -0500 Subject: [PATCH 098/152] Switch a couple more catch blocks to const. Signed-off-by: Chris Lalancette --- ublox_gps/src/raw_data_pa.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 071cdda7..f8a34e2d 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -134,8 +134,8 @@ void RawDataStreamPa::initialize() { try { file_handle_.open(file_name_); RCLCPP_INFO(this->get_logger(), "Logging raw data to file \"%s\"", - file_name_.c_str()); - } catch(const std::exception& e) { + file_name_.c_str()); + } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " "Can't create file \"%s\".", file_name_.c_str()); } @@ -191,7 +191,7 @@ void RawDataStreamPa::saveToFile(const std::string & str) { try { file_handle_ << str; // file_handle_.flush(); - } catch(const std::exception& e) { + } catch (const std::exception& e) { RCLCPP_WARN(this->get_logger(), "Error writing to file \"%s\"", file_name_.c_str()); } } From 30fa1c7f8f92d83bd13139505ccec43934c01d0d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Dec 2019 11:22:24 -0500 Subject: [PATCH 099/152] Re-enable logging in the AsyncWorker. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 35 +++++++++++--------- ublox_gps/src/gps.cpp | 7 ++-- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index bb7315b4..da681712 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -61,8 +61,9 @@ class AsyncWorker final : public Worker { */ explicit AsyncWorker(std::shared_ptr stream, std::shared_ptr io_service, - std::size_t buffer_size = 8192, - int debug = 1); + std::size_t buffer_size, + int debug, + const rclcpp::Logger& logger); ~AsyncWorker(); /** @@ -129,24 +130,24 @@ class AsyncWorker final : public Worker { std::shared_ptr background_thread_; //!< thread for the I/O //!< service - Callback read_callback_; //!< Callback function to handle received messages - Callback write_callback_; //!< Callback function to handle raw data + WorkerCallback read_callback_; //!< Callback function to handle received messages + WorkerRawCallback raw_callback_; //!< Callback function to handle raw data bool stopping_; //!< Whether or not the I/O service is closed int debug_; //!< Used to determine which debug messages to display + + rclcpp::Logger logger_; }; template AsyncWorker::AsyncWorker(std::shared_ptr stream, std::shared_ptr io_service, std::size_t buffer_size, - int debug) - : stopping_(false), debug_(debug) { - stream_ = stream; - io_service_ = io_service; + int debug, + const rclcpp::Logger& logger) + : stream_(stream), io_service_(io_service), in_buffer_size_(0), stopping_(false), debug_(debug), logger_(logger) { in_.resize(buffer_size); - in_buffer_size_ = 0; out_.reserve(buffer_size); @@ -166,12 +167,12 @@ bool AsyncWorker::send(const unsigned char* data, const unsigned int size) { std::lock_guard lock(write_mutex_); if (size == 0) { - // RCLCPP_ERROR("Ublox AsyncWorker::send: Size of message to send is 0"); + RCLCPP_ERROR(logger_, "Ublox AsyncWorker::send: Size of message to send is 0"); return true; } if (out_.capacity() - out_.size() < size) { - // RCLCPP_ERROR("Ublox AsyncWorker::send: Output buffer too full to send message"); + RCLCPP_ERROR(logger_, "Ublox AsyncWorker::send: Output buffer too full to send message"); return false; } out_.insert(out_.end(), data, data + size); @@ -197,7 +198,7 @@ void AsyncWorker::doWrite() { it != out_.end(); ++it) { oss << std::hex << static_cast(*it) << " "; } - // RCLCPP_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); + RCLCPP_DEBUG(logger_, "U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); } // Clear the buffer & unlock out_.clear(); @@ -239,8 +240,8 @@ void AsyncWorker::readEnd(const asio::error_code& error, it != in_.begin() + in_buffer_size_; ++it) { oss << std::hex << static_cast(*it) << " "; } - // RCLCPP_DEBUG("U-Blox received %li bytes \n%s", bytes_transfered, - // oss.str().c_str()); + RCLCPP_DEBUG(logger_, "U-Blox received %li bytes \n%s", bytes_transferred, + oss.str().c_str()); } if (read_callback_) { @@ -248,6 +249,8 @@ void AsyncWorker::readEnd(const asio::error_code& error, } read_condition_.notify_all(); + } else { + RCLCPP_ERROR(logger_, "U-Blox ASIO transferred zero bytes"); } if (!stopping_) { @@ -262,8 +265,8 @@ void AsyncWorker::doClose() { asio::error_code error; stream_->close(error); if (error) { - // RCLCPP_ERROR_STREAM( - // "Error while closing the AsyncWorker stream: " << error.message()); + RCLCPP_ERROR(logger_, "Error while closing the AsyncWorker stream: %s", + error.message().c_str()); } } diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 79779534..c55dc02f 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -147,7 +147,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service, 8192, debug_)); + setWorker(std::make_shared>(serial, io_service, 8192, debug_, logger_)); configured_ = false; @@ -197,7 +197,7 @@ void Gps::resetSerial(const std::string & port) { if (worker_) { return; } - setWorker(std::make_shared>(serial, io_service, 8192, debug_)); + setWorker(std::make_shared>(serial, io_service, 8192, debug_, logger_)); configured_ = false; // Poll UART PRT Config @@ -250,8 +250,7 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { if (worker_) { return; } - setWorker(std::make_shared>(socket, - io_service, 8192, debug_)); + setWorker(std::make_shared>(socket, io_service, 8192, debug_, logger_)); } void Gps::close() { From 08328d688cf1082ac039a4c7c262da68eb3c1035 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Dec 2019 11:25:22 -0500 Subject: [PATCH 100/152] Fix a bug for reads of zero size. It turns out that the U-blox can sometimes send us messages of a bogus length that are way too long. In those cases, we could be asking for 0 byte reads, which ASIO will happily do and then immediately return 0 bytes. Instead of this, if we detect that we would have asked for zero bytes, just throw away the entire message and do a full read. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 42 ++++++++++++-------- ublox_gps/include/ublox_gps/callback.hpp | 6 ++- ublox_gps/include/ublox_gps/gps.hpp | 2 +- ublox_gps/include/ublox_gps/worker.hpp | 7 ++-- ublox_gps/src/gps.cpp | 2 +- 5 files changed, 36 insertions(+), 23 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index da681712..553d3604 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -70,13 +70,13 @@ class AsyncWorker final : public Worker { * @brief Set the callback function which handles input messages. * @param callback the read callback which handles received messages */ - void setCallback(const Callback& callback) override { read_callback_ = callback; } + void setCallback(const WorkerCallback& callback) override { read_callback_ = callback; } /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Callback& callback) override { write_callback_ = callback; } + void setRawDataCallback(const WorkerRawCallback& callback) override { raw_callback_ = callback; } /** * @brief Send the data bytes via the I/O stream. @@ -208,35 +208,45 @@ void AsyncWorker::doWrite() { template void AsyncWorker::doRead() { std::lock_guard lock(read_mutex_); + if (in_.size() - in_buffer_size_ == 0) { + // In some circumstances, it is possible that there is no room left in the + // buffer. This can happen, for instance, if one of the UBlox messages + // has a value in the Length field that is much larger than this buffer + // can accomodate. We definitely don't want to ask for a 0-byte read (as + // we will get into an endless loop of asking for, and then receiving, + // 0 bytes), so we just throw away all of the data in the buffer. + in_buffer_size_ = 0; + } + stream_->async_read_some( asio::buffer(in_.data() + in_buffer_size_, in_.size() - in_buffer_size_), - std::bind(&AsyncWorker::readEnd, this, - std::placeholders::_1, std::placeholders::_2)); + std::bind(&AsyncWorker::readEnd, this, + std::placeholders::_1, std::placeholders::_2)); } template void AsyncWorker::readEnd(const asio::error_code& error, - std::size_t bytes_transfered) { + std::size_t bytes_transferred) { std::lock_guard lock(read_mutex_); if (error) { - // RCLCPP_ERROR("U-Blox ASIO input buffer read error: %s, %li", - // error.message().c_str(), - // bytes_transfered); - } else if (bytes_transfered > 0) { - in_buffer_size_ += bytes_transfered; + RCLCPP_ERROR(logger_, "U-Blox ASIO input buffer read error: %s, %li", + error.message().c_str(), + bytes_transferred); + } else if (bytes_transferred > 0) { + in_buffer_size_ += bytes_transferred; - unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transfered))); - std::size_t raw_data_stream_size = bytes_transfered; + unsigned char *pRawDataStart = &(*(in_.begin() + (in_buffer_size_ - bytes_transferred))); + std::size_t raw_data_stream_size = bytes_transferred; - if (write_callback_) { - write_callback_(pRawDataStart, raw_data_stream_size); + if (raw_callback_) { + raw_callback_(pRawDataStart, raw_data_stream_size); } if (debug_ >= 4) { std::ostringstream oss; for (std::vector::iterator it = - in_.begin() + in_buffer_size_ - bytes_transfered; + in_.begin() + in_buffer_size_ - bytes_transferred; it != in_.begin() + in_buffer_size_; ++it) { oss << std::hex << static_cast(*it) << " "; } @@ -245,7 +255,7 @@ void AsyncWorker::readEnd(const asio::error_code& error, } if (read_callback_) { - read_callback_(in_.data(), in_buffer_size_); + in_buffer_size_ -= read_callback_(in_.data(), in_buffer_size_); } read_condition_.notify_all(); diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index fe49c5de..2b0a00fb 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -209,8 +209,9 @@ class CallbackHandlers final { * messages from the buffer. * @param data the buffer of u-blox messages to process * @param size the size of the buffer + * @return the number of bytes consumed */ - void readCallback(unsigned char* data, std::size_t& size) { + size_t readCallback(unsigned char* data, std::size_t size) { ublox::Reader reader(data, size); // Read all U-Blox messages in buffer while (reader.search() != reader.end() && reader.found()) { @@ -230,7 +231,8 @@ class CallbackHandlers final { // delete read bytes from ASIO input buffer std::copy(reader.pos(), reader.end(), data); - size -= reader.pos() - data; + + return reader.pos() - data; } private: diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index fca143ec..06fbb982 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -402,7 +402,7 @@ class Gps final { * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - void setRawDataCallback(const Worker::Callback& callback); + void setRawDataCallback(const Worker::WorkerRawCallback& callback); private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK diff --git a/ublox_gps/include/ublox_gps/worker.hpp b/ublox_gps/include/ublox_gps/worker.hpp index da1c87ce..8af38976 100644 --- a/ublox_gps/include/ublox_gps/worker.hpp +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -39,20 +39,21 @@ namespace ublox_gps { */ class Worker { public: - typedef std::function Callback; + typedef std::function WorkerCallback; + typedef std::function WorkerRawCallback; virtual ~Worker() {} /** * @brief Set the callback function for received messages. * @param callback the callback function which process messages in the buffer */ - virtual void setCallback(const Callback& callback) = 0; + virtual void setCallback(const WorkerCallback& callback) = 0; /** * @brief Set the callback function which handles raw data. * @param callback the write callback which handles raw data */ - virtual void setRawDataCallback(const Callback& callback) = 0; + virtual void setRawDataCallback(const WorkerRawCallback& callback) = 0; /** * @brief Send the data in the buffer. diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index c55dc02f..f451b7be 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -581,7 +581,7 @@ bool Gps::waitForAcknowledge(const std::chrono::milliseconds& timeout, return result; } -void Gps::setRawDataCallback(const Worker::Callback& callback) { +void Gps::setRawDataCallback(const Worker::WorkerRawCallback& callback) { if (!worker_) { return; } From b71d85813e51a5c142cc37c89921ecdbb17437e6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Dec 2019 11:26:45 -0500 Subject: [PATCH 101/152] Rename rawDataStreamPa member variable to raw_data_stream_pa Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/node.hpp | 2 +- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 5 ++--- ublox_gps/src/node.cpp | 8 ++++---- ublox_gps/src/raw_data_pa.cpp | 10 +++++----- 4 files changed, 12 insertions(+), 13 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 613cae28..81804514 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -238,7 +238,7 @@ class UbloxNode final : public rclcpp::Node { uint8_t tim_rate_; //! raw data stream logging - RawDataStreamPa rawDataStreamPa_; + RawDataStreamPa raw_data_stream_pa_; rclcpp::Publisher::SharedPtr nav_status_pub_; rclcpp::Publisher::SharedPtr nav_posecef_pub_; diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 0a4d2185..721f841f 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -59,7 +59,6 @@ namespace ublox_node { class RawDataStreamPa final : public rclcpp::Node { public: - /** * @brief Constructor. * Initialises variables and the nodehandle. @@ -91,7 +90,7 @@ class RawDataStreamPa final : public rclcpp::Node { * @param size the size of the buffer */ void ubloxCallback(const unsigned char* data, - const std::size_t size); + std::size_t size); private: /** @@ -117,7 +116,7 @@ class RawDataStreamPa final : public rclcpp::Node { */ void saveToFile(const std::string & str); - //! Directoy name for storing raw data + //! Directory name for storing raw data std::string file_dir_; //! Filename for storing raw data std::string file_name_; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index e080b41f..a94fb1fa 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -357,7 +357,7 @@ void UbloxNode::getRosParams() { this->declare_parameter("dgnss_mode"); // raw data stream logging - rawDataStreamPa_.getRosParams(); + raw_data_stream_pa_.getRosParams(); // NMEA parameters this->declare_parameter("nmea.set", false); @@ -761,10 +761,10 @@ void UbloxNode::initializeIo() { } // raw data stream logging - if (rawDataStreamPa_.isEnabled()) { + if (raw_data_stream_pa_.isEnabled()) { gps_->setRawDataCallback( - std::bind(&RawDataStreamPa::ubloxCallback, &rawDataStreamPa_, std::placeholders::_1, std::placeholders::_2)); - rawDataStreamPa_.initialize(); + std::bind(&RawDataStreamPa::ubloxCallback, &raw_data_stream_pa_, std::placeholders::_1, std::placeholders::_2)); + raw_data_stream_pa_.initialize(); } } diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index f8a34e2d..08b54b85 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -98,13 +98,13 @@ void RawDataStreamPa::initialize() { if (!file_dir_.empty()) { struct stat stat_info; - if (::stat(file_dir_.c_str(), &stat_info ) != 0) { + if (::stat(file_dir_.c_str(), &stat_info) != 0) { RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " - "Directory \"%s\" does not exist.", file_dir_.c_str()); + "Directory \"%s\" does not exist.", file_dir_.c_str()); } else if ((stat_info.st_mode & S_IFDIR) != S_IFDIR) { RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " - "\"%s\" exists, but is not a directory.", file_dir_.c_str()); + "\"%s\" exists, but is not a directory.", file_dir_.c_str()); } else { if (file_dir_.back() != '/') { @@ -137,14 +137,14 @@ void RawDataStreamPa::initialize() { file_name_.c_str()); } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " - "Can't create file \"%s\".", file_name_.c_str()); + "Can't create file \"%s\".", file_name_.c_str()); } } } } void RawDataStreamPa::ubloxCallback(const unsigned char* data, - const std::size_t size) { + std::size_t size) { std::string str((const char*) data, size); From 22b5ac8b32cd9a38c787fc40af3e5d821a7e9815 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Dec 2019 16:08:53 -0500 Subject: [PATCH 102/152] Make sure to initialize checksum properly. Signed-off-by: Chris Lalancette --- .../include/ublox_serialization/serialization.hpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 7dcfc666..3dce5c15 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -490,11 +490,18 @@ class Reader { return false; } - uint16_t chk; + uint16_t chk{0}; if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) { // checksum error - // ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), - // messageId()); + // Note that it is possible (and even likely) that we get here without + // having the entire packet available. This happens when there are both + // NMEA and UBlox messages configured on the serial wire, and the packet is + // laid out like: + // + // + // + // Therefore, we do not print errors in this case and instead just don't + // do any additional work. return false; } From 1c64443b48082f2610c87485712fb296f2959fc1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Jan 2020 10:35:06 -0500 Subject: [PATCH 103/152] Small style fixups. Signed-off-by: Chris Lalancette --- ublox_msgs/msg/CfgRATE.msg | 6 +++--- ublox_msgs/msg/NavPVT.msg | 16 ++++++++-------- ublox_msgs/msg/NavPVT7.msg | 18 +++++++++--------- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ublox_msgs/msg/CfgRATE.msg b/ublox_msgs/msg/CfgRATE.msg index 23b7e55d..dc89f34c 100644 --- a/ublox_msgs/msg/CfgRATE.msg +++ b/ublox_msgs/msg/CfgRATE.msg @@ -21,10 +21,10 @@ uint8 CLASS_ID = 6 uint8 MESSAGE_ID = 8 uint16 meas_rate # Measurement Rate, GPS measurements are - # taken every measRate milliseconds [ms] + # taken every measRate milliseconds [ms] uint16 nav_rate # Navigation Rate, in number of measurement - # cycles. On u-blox 5 and u-blox 6, this parameter - # cannot be changed, and always equals 1. + # cycles. On u-blox 5 and u-blox 6, this parameter + # cannot be changed, and always equals 1. uint16 time_ref # The time system to which measurements are aligned uint16 TIME_REF_UTC = 0 uint16 TIME_REF_GPS = 1 diff --git a/ublox_msgs/msg/NavPVT.msg b/ublox_msgs/msg/NavPVT.msg index 847b3bce..eed12ad1 100644 --- a/ublox_msgs/msg/NavPVT.msg +++ b/ublox_msgs/msg/NavPVT.msg @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 i_tow # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -28,7 +28,7 @@ uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 t_acc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) uint8 fix_type # GNSS fix Type, range 0..5 @@ -68,9 +68,9 @@ uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed uint8 num_sv # Number of SVs used in Nav Solution -int32 lon # Longitude [deg / 1e-7] -int32 lat # Latitude [deg / 1e-7] -int32 height # Height above Ellipsoid [mm] +int32 lon # Longitude [deg / 1e-7] +int32 lat # Latitude [deg / 1e-7] +int32 height # Height above Ellipsoid [mm] int32 h_msl # Height above mean sea level [mm] uint32 h_acc # Horizontal Accuracy Estimate [mm] uint32 v_acc # Vertical Accuracy Estimate [mm] @@ -79,13 +79,13 @@ int32 vel_n # NED north velocity [mm/s] int32 vel_e # NED east velocity [mm/s] int32 vel_d # NED down velocity [mm/s] int32 g_speed # Ground Speed (2-D) [mm/s] -int32 heading # Heading of motion 2-D [deg / 1e-5] +int32 heading # Heading of motion 2-D [deg / 1e-5] uint32 s_acc # Speed Accuracy Estimate [mm/s] uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) - # [deg / 1e-5] + # [deg / 1e-5] uint16 p_dop # Position DOP [1 / 0.01] -uint8[6] reserved1 # Reserved +uint8[6] reserved1 # Reserved int32 head_veh # Heading of vehicle (2-D) [deg / 1e-5] int16 mag_dec # Magnetic declination [deg / 1e-2] diff --git a/ublox_msgs/msg/NavPVT7.msg b/ublox_msgs/msg/NavPVT7.msg index 6ec136e6..97f7a09c 100644 --- a/ublox_msgs/msg/NavPVT7.msg +++ b/ublox_msgs/msg/NavPVT7.msg @@ -13,7 +13,7 @@ uint8 CLASS_ID = 1 uint8 MESSAGE_ID = 7 -uint32 i_tow # GPS Millisecond time of week [ms] +uint32 i_tow # GPS Millisecond time of week [ms] uint16 year # Year (UTC) uint8 month # Month, range 1..12 (UTC) uint8 day # Day of month, range 1..31 (UTC) @@ -28,7 +28,7 @@ uint8 VALID_FULLY_RESOLVED = 4 # UTC time of day has been fully resolved # (no seconds uncertainty) uint8 VALID_MAG = 8 # Valid Magnetic Declination -uint32 t_acc # time accuracy estimate [ns] (UTC) +uint32 t_acc # time accuracy estimate [ns] (UTC) int32 nano # fraction of a second [ns], range -1e9 .. 1e9 (UTC) uint8 fix_type # GNSS fix Type, range 0..5 @@ -52,7 +52,7 @@ uint8 PSM_TRACKING = 12 # Tracking uint8 PSM_POWER_OPTIMIZED_TRACKING = 16 # Power Optimized Tracking uint8 PSM_INACTIVE = 20 # Inactive uint8 FLAGS_HEAD_VEH_VALID = 32 # heading of vehicle is valid -uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status +uint8 FLAGS_CARRIER_PHASE_MASK = 192 # Carrier Phase Range Solution Status uint8 CARRIER_PHASE_NO_SOLUTION = 0 # no carrier phase range solution uint8 CARRIER_PHASE_FLOAT = 64 # carrier phase float solution (no fixed # integer measurements have been used to @@ -68,9 +68,9 @@ uint8 FLAGS2_CONFIRMED_DATE = 64 # UTC Date validity could be confirmed uint8 FLAGS2_CONFIRMED_TIME = 128 # UTC Time of Day could be confirmed uint8 num_sv # Number of SVs used in Nav Solution -int32 lon # Longitude [deg / 1e-7] -int32 lat # Latitude [deg / 1e-7] -int32 height # Height above Ellipsoid [mm] +int32 lon # Longitude [deg / 1e-7] +int32 lat # Latitude [deg / 1e-7] +int32 height # Height above Ellipsoid [mm] int32 h_msl # Height above mean sea level [mm] uint32 h_acc # Horizontal Accuracy Estimate [mm] uint32 v_acc # Vertical Accuracy Estimate [mm] @@ -79,10 +79,10 @@ int32 vel_n # NED north velocity [mm/s] int32 vel_e # NED east velocity [mm/s] int32 vel_d # NED down velocity [mm/s] int32 g_speed # Ground Speed (2-D) [mm/s] -int32 heading # Heading of motion 2-D [deg / 1e-5] +int32 heading # Heading of motion 2-D [deg / 1e-5] uint32 s_acc # Speed Accuracy Estimate [mm/s] uint32 head_acc # Heading Accuracy Estimate (both motion & vehicle) - # [deg / 1e-5] + # [deg / 1e-5] uint16 p_dop # Position DOP [1 / 0.01] -uint8[6] reserved1 # Reserved +uint8[6] reserved1 # Reserved From 1deaf66e254d6e6e7fff0047a79ff9c9525ef8a2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Jan 2020 13:44:47 -0500 Subject: [PATCH 104/152] Fixes pointed out by clang-tidy. Signed-off-by: Chris Lalancette --- ublox_gps/include/ublox_gps/async_worker.hpp | 13 +- ublox_gps/include/ublox_gps/callback.hpp | 9 +- .../include/ublox_gps/component_interface.hpp | 2 +- .../include/ublox_gps/fix_diagnostic.hpp | 21 ++- ublox_gps/include/ublox_gps/fts_product.hpp | 2 +- ublox_gps/include/ublox_gps/gnss.hpp | 11 +- ublox_gps/include/ublox_gps/gps.hpp | 29 ++-- .../include/ublox_gps/hpg_rov_product.hpp | 10 +- ublox_gps/include/ublox_gps/node.hpp | 43 +++--- ublox_gps/include/ublox_gps/raw_data_pa.hpp | 4 +- ublox_gps/include/ublox_gps/rtcm.hpp | 4 +- .../include/ublox_gps/ublox_firmware7plus.hpp | 20 +-- .../include/ublox_gps/ublox_firmware8.hpp | 10 +- .../ublox_gps/ublox_topic_diagnostic.hpp | 13 +- ublox_gps/include/ublox_gps/utils.hpp | 9 +- ublox_gps/include/ublox_gps/worker.hpp | 15 +- ublox_gps/src/adr_udr_product.cpp | 12 +- ublox_gps/src/gnss.cpp | 8 - ublox_gps/src/gps.cpp | 65 ++++---- ublox_gps/src/hpg_ref_product.cpp | 19 ++- ublox_gps/src/hpg_rov_product.cpp | 22 +-- ublox_gps/src/mkgmtime.c | 120 +++++++------- ublox_gps/src/node.cpp | 146 ++++++++++-------- ublox_gps/src/raw_data_pa.cpp | 12 +- ublox_gps/src/ublox_firmware6.cpp | 26 ++-- ublox_gps/src/ublox_firmware7.cpp | 46 +++--- ublox_gps/src/ublox_firmware8.cpp | 80 +++++----- ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 30 ++-- .../ublox_serialization/serialization.hpp | 22 ++- 29 files changed, 440 insertions(+), 383 deletions(-) diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 553d3604..2185e78c 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -43,6 +43,8 @@ #include #include +#include + #include "worker.hpp" namespace ublox_gps { @@ -64,7 +66,12 @@ class AsyncWorker final : public Worker { std::size_t buffer_size, int debug, const rclcpp::Logger& logger); - ~AsyncWorker(); + ~AsyncWorker() override; + + AsyncWorker(AsyncWorker &&c) = delete; + AsyncWorker &operator=(AsyncWorker &&c) = delete; + AsyncWorker(const AsyncWorker &c) = delete; + AsyncWorker &operator=(const AsyncWorker &c) = delete; /** * @brief Set the callback function which handles input messages. @@ -101,9 +108,9 @@ class AsyncWorker final : public Worker { /** * @brief Process messages read from the input stream. * @param error_code an error code for read failures - * @param the number of bytes received + * @param bytes_received the number of bytes received */ - void readEnd(const asio::error_code&, std::size_t); + void readEnd(const asio::error_code& error, std::size_t bytes_transferred); /** * @brief Send all the data in the output buffer. diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 2b0a00fb..01db8126 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -32,8 +32,11 @@ #include #include #include +#include +#include #include #include +#include #include #include @@ -71,7 +74,7 @@ class CallbackHandler { template class CallbackHandler_ final : public CallbackHandler { public: - typedef std::function Callback; //!< A callback function + using Callback = std::function; /** * @brief Initialize the Callback Handler with a callback function @@ -236,8 +239,8 @@ class CallbackHandlers final { } private: - typedef std::multimap, - std::shared_ptr > Callbacks; + using Callbacks = std::multimap, + std::shared_ptr>; // Call back handlers for u-blox messages Callbacks callbacks_; diff --git a/ublox_gps/include/ublox_gps/component_interface.hpp b/ublox_gps/include/ublox_gps/component_interface.hpp index 72d8ab92..951f4e2e 100644 --- a/ublox_gps/include/ublox_gps/component_interface.hpp +++ b/ublox_gps/include/ublox_gps/component_interface.hpp @@ -75,6 +75,6 @@ class ComponentInterface { virtual void subscribe(std::shared_ptr gps) = 0; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp index 3c9d7b1d..556b1ba7 100644 --- a/ublox_gps/include/ublox_gps/fix_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/fix_diagnostic.hpp @@ -10,12 +10,8 @@ namespace ublox_node { //! Topic diagnostics for fix / fix_velocity messages -struct FixDiagnostic { - FixDiagnostic() {} - - // Must not copy this struct (would confuse FrequencyStatusParam pointers) - FixDiagnostic(const FixDiagnostic&) = delete; - +class FixDiagnostic { +public: /** * @brief Add a topic diagnostic to the diagnostic updater for fix topics. * @@ -29,7 +25,6 @@ struct FixDiagnostic { double stamp_min, uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater) { const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz - //const double target_freq = 1.0; min_freq = target_freq; max_freq = target_freq; diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, @@ -42,14 +37,24 @@ struct FixDiagnostic { time_param); } + // Must not copy this struct (would confuse FrequencyStatusParam pointers) + FixDiagnostic(FixDiagnostic &&c) = delete; + FixDiagnostic &operator=(FixDiagnostic &&c) = delete; + FixDiagnostic(const FixDiagnostic &c) = delete; + FixDiagnostic &operator=(const FixDiagnostic &c) = delete; + + ~FixDiagnostic() = default; + //! Topic frequency diagnostic updater std::shared_ptr diagnostic; + +private: //! Minimum allow frequency of topic double min_freq; //! Maximum allow frequency of topic double max_freq; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/fts_product.hpp b/ublox_gps/include/ublox_gps/fts_product.hpp index d66acbbd..fed2c843 100644 --- a/ublox_gps/include/ublox_gps/fts_product.hpp +++ b/ublox_gps/include/ublox_gps/fts_product.hpp @@ -75,6 +75,6 @@ class FtsProduct final : public virtual ComponentInterface { } }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/gnss.hpp b/ublox_gps/include/ublox_gps/gnss.hpp index 0d3e61ad..799a1227 100644 --- a/ublox_gps/include/ublox_gps/gnss.hpp +++ b/ublox_gps/include/ublox_gps/gnss.hpp @@ -8,8 +8,13 @@ namespace ublox_node { class Gnss final { public: - Gnss(); - ~Gnss(); + Gnss() = default; + ~Gnss() = default; + + Gnss(Gnss &&c) = delete; + Gnss &operator=(Gnss &&c) = delete; + Gnss(const Gnss &c) = delete; + Gnss &operator=(const Gnss &c) = delete; void add(const std::string & gnss); bool isSupported(const std::string & gnss); @@ -18,6 +23,6 @@ class Gnss final { std::set supported_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 06fbb982..003aa31b 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -53,14 +53,14 @@ */ namespace ublox_gps { //! Possible baudrates for u-blox devices -constexpr static unsigned int kBaudrates[] = { 4800, - 9600, - 19200, - 38400, - 57600, - 115200, - 230400, - 460800 }; +const std::vector kBaudrates{ 4800, + 9600, + 19200, + 38400, + 57600, + 115200, + 230400, + 460800 }; /** * @brief Handles communication with and configuration of the u-blox device */ @@ -76,6 +76,11 @@ class Gps final { explicit Gps(int debug, const rclcpp::Logger & logger); ~Gps(); + Gps(Gps &&c) = delete; + Gps &operator=(Gps &&c) = delete; + Gps(const Gps &c) = delete; + Gps &operator=(const Gps &c) = delete; + /** * @brief If called, when the node shuts down, it will send a command to * save the flash memory. @@ -88,7 +93,7 @@ class Gps final { * @brief Set the internal flag for enabling or disabling the initial configurations. * @param config_on_startup boolean flag */ - void setConfigOnStartup(const bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } + void setConfigOnStartup(bool config_on_startup) { config_on_startup_flag_ = config_on_startup; } /** * @brief Initialize TCP I/O. @@ -170,7 +175,7 @@ class Gps final { * configuration parameters * @return true on ACK, false on other conditions. */ - bool disableUart1(ublox_msgs::msg::CfgPRT& prev_cfg); + bool disableUart1(ublox_msgs::msg::CfgPRT& prev_config); /** * @brief Configure the USB Port. @@ -352,7 +357,7 @@ class Gps final { bool read(T& message, const std::chrono::milliseconds& timeout = default_timeout_); - bool isInitialized() const { return worker_ != 0; } + bool isInitialized() const { return worker_ != nullptr; } bool isConfigured() const { return isInitialized() && configured_; } bool isOpen() const { return worker_->isOpen(); } @@ -530,7 +535,7 @@ bool Gps::configure(const ConfigT& message, bool wait) { } // Reset ack - Ack ack; + Ack ack{}; ack.type = WAIT; ack.class_id = 0; ack.msg_id = 0; diff --git a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp index 07d660c5..2474e1c9 100644 --- a/ublox_gps/include/ublox_gps/hpg_rov_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_rov_product.hpp @@ -21,13 +21,13 @@ class HpgRovProduct final : public virtual ComponentInterface { public: // Constants for diagnostic updater //! Diagnostic updater: RTCM topic frequency min [Hz] - constexpr static double kRtcmFreqMin = 1; + const double kRtcmFreqMin = 1; //! Diagnostic updater: RTCM topic frequency max [Hz] - constexpr static double kRtcmFreqMax = 10; + const double kRtcmFreqMax = 10; //! Diagnostic updater: RTCM topic frequency tolerance [%] - constexpr static double kRtcmFreqTol = 0.1; + const double kRtcmFreqTol = 0.1; //! Diagnostic updater: RTCM topic frequency window [num messages] - constexpr static int kRtcmFreqWindow = 25; + const int kRtcmFreqWindow = 25; explicit HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node); @@ -81,7 +81,7 @@ class HpgRovProduct final : public virtual ComponentInterface { uint8_t dgnss_mode_; //! The RTCM topic frequency diagnostic updater - UbloxTopicDiagnostic freq_rtcm_; + std::unique_ptr freq_rtcm_; rclcpp::Publisher::SharedPtr nav_rel_pos_ned_pub_; diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 81804514..a2745711 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -99,7 +99,12 @@ class UbloxNode final : public rclcpp::Node { */ UbloxNode(); - ~UbloxNode(); + ~UbloxNode() override; + + UbloxNode(UbloxNode &&c) = delete; + UbloxNode &operator=(UbloxNode &&c) = delete; + UbloxNode(const UbloxNode &c) = delete; + UbloxNode &operator=(const UbloxNode &c) = delete; /** * @brief Get the node parameters from the ROS Parameter Server. @@ -193,7 +198,7 @@ class UbloxNode final : public rclcpp::Node { std::vector > components_; //! Determined From Mon VER - float protocol_version_ = 0; + float protocol_version_ = 0.0; // Variables set from parameter server //! Device port std::string device_; @@ -202,40 +207,40 @@ class UbloxNode final : public rclcpp::Node { //! Fix mode type std::string fix_mode_; //! Set from dynamic model string - uint8_t dmodel_; + uint8_t dmodel_{0}; //! Set from fix mode string - uint8_t fmode_; + uint8_t fmode_{0}; //! UART1 baudrate - uint32_t baudrate_; + uint32_t baudrate_{0}; //! UART in protocol (see CfgPRT message for constants) - uint16_t uart_in_; + uint16_t uart_in_{0}; //! UART out protocol (see CfgPRT message for constants) - uint16_t uart_out_; + uint16_t uart_out_{0}; //! USB TX Ready Pin configuration (see CfgPRT message for constants) - uint16_t usb_tx_; + uint16_t usb_tx_{0}; //! Whether to configure the USB port /*! Set to true if usb_in & usb_out parameters are set */ - bool set_usb_; + bool set_usb_{false}; //! USB in protocol (see CfgPRT message for constants) - uint16_t usb_in_; + uint16_t usb_in_{0}; //! USB out protocol (see CfgPRT message for constants) - uint16_t usb_out_ ; + uint16_t usb_out_{0}; //! The measurement rate in Hz - double rate_; + double rate_{0.0}; //! User-defined Datum ublox_msgs::msg::CfgDAT cfg_dat_; //! SBAS Usage parameter (see CfgSBAS message) - uint8_t sbas_usage_; + uint8_t sbas_usage_{0}; //! Max SBAS parameter (see CfgSBAS message) - uint8_t max_sbas_; + uint8_t max_sbas_{0}; //! Dead reckoning limit parameter - uint8_t dr_limit_; + uint8_t dr_limit_{0}; //! Parameters to load from non-volatile memory during configuration ublox_msgs::msg::CfgCFG load_; //! Parameters to save to non-volatile memory after configuration ublox_msgs::msg::CfgCFG save_; //! rate for TIM-TM2 - uint8_t tim_rate_; + uint8_t tim_rate_{0}; //! raw data stream logging RawDataStreamPa raw_data_stream_pa_; @@ -248,10 +253,10 @@ class UbloxNode final : public rclcpp::Node { rclcpp::Publisher::SharedPtr aid_hui_pub_; //! Navigation rate in measurement cycles, see CfgRate.msg - uint16_t nav_rate_; + uint16_t nav_rate_{0}; //! The measurement [ms], see CfgRate.msg - uint16_t meas_rate_; + uint16_t meas_rate_{0}; //! The ROS frame ID of this device std::string frame_id_; @@ -273,6 +278,6 @@ class UbloxNode final : public rclcpp::Node { rclcpp::TimerBase::SharedPtr poller_; }; -} +} // namespace ublox_node #endif // UBLOX_GPS_NODE_HPP diff --git a/ublox_gps/include/ublox_gps/raw_data_pa.hpp b/ublox_gps/include/ublox_gps/raw_data_pa.hpp index 721f841f..24d13d92 100644 --- a/ublox_gps/include/ublox_gps/raw_data_pa.hpp +++ b/ublox_gps/include/ublox_gps/raw_data_pa.hpp @@ -63,7 +63,7 @@ class RawDataStreamPa final : public rclcpp::Node { * @brief Constructor. * Initialises variables and the nodehandle. */ - RawDataStreamPa(bool is_ros_subscriber = false); + explicit RawDataStreamPa(bool is_ros_subscriber = false); /** * @brief Get the raw data stream parameters. @@ -135,6 +135,6 @@ class RawDataStreamPa final : public rclcpp::Node { rclcpp::Subscription::SharedPtr raw_data_stream_sub_; }; -} +} // namespace ublox_node #endif diff --git a/ublox_gps/include/ublox_gps/rtcm.hpp b/ublox_gps/include/ublox_gps/rtcm.hpp index ee39bc4e..689faa20 100644 --- a/ublox_gps/include/ublox_gps/rtcm.hpp +++ b/ublox_gps/include/ublox_gps/rtcm.hpp @@ -1,6 +1,8 @@ #ifndef UBLOX_GPS_RTCM_HPP #define UBLOX_GPS_RTCM_HPP +#include + namespace ublox_gps { struct Rtcm { @@ -10,6 +12,6 @@ struct Rtcm { uint8_t rate; }; -} // namespace ublox_node +} // namespace ublox_gps #endif diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp index 7020a84b..f18fae1b 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -72,11 +72,11 @@ class UbloxFirmware7Plus : public UbloxFirmware { // converted to a positive value if (m.nano < 0) { fix.header.stamp.sec = toUtcSeconds(m) - 1; - fix.header.stamp.nanosec = (uint32_t)(m.nano + 1e9); + fix.header.stamp.nanosec = static_cast(m.nano + 1e9); } else { fix.header.stamp.sec = toUtcSeconds(m); - fix.header.stamp.nanosec = (uint32_t)(m.nano); + fix.header.stamp.nanosec = static_cast(m.nano); } } else { // Use ROS time since NavPVT timestamp is not valid @@ -89,12 +89,12 @@ class UbloxFirmware7Plus : public UbloxFirmware { // Set the Fix status bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK; if (fixOk && m.fix_type >= m.FIX_TYPE_2D) { - fix.status.status = fix.status.STATUS_FIX; + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; if (m.flags & m.CARRIER_PHASE_FIXED) { - fix.status.status = fix.status.STATUS_GBAS_FIX; + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX; } } else { - fix.status.status = fix.status.STATUS_NO_FIX; + fix.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; } // Set the service based on GNSS configuration fix.status.service = fix_status_service_; @@ -185,20 +185,20 @@ class UbloxFirmware7Plus : public UbloxFirmware { stat.add("Height above MSL [m]", last_nav_pvt_.h_msl * 1e-3); stat.add("Horizontal Accuracy [m]", last_nav_pvt_.h_acc * 1e-3); stat.add("Vertical Accuracy [m]", last_nav_pvt_.v_acc * 1e-3); - stat.add("# SVs used", (int)last_nav_pvt_.num_sv); + stat.add("# SVs used", static_cast(last_nav_pvt_.num_sv)); } //! The last received NavPVT message NavPVT last_nav_pvt_; // Whether or not to enable the given GNSS //! Whether or not to enable GPS - bool enable_gps_; + bool enable_gps_{false}; //! Whether or not to enable GLONASS - bool enable_glonass_; + bool enable_glonass_{false}; //! Whether or not to enable QZSS - bool enable_qzss_; + bool enable_qzss_{false}; //! The QZSS Signal configuration, see CfgGNSS message - uint32_t qzss_sig_cfg_; + uint32_t qzss_sig_cfg_{0}; typename rclcpp::Publisher::SharedPtr nav_pvt_pub_; rclcpp::Publisher::SharedPtr fix_pub_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index 0d1158c6..0f21db5a 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -58,16 +58,16 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { private: // Set from ROS parameters //! Whether or not to enable the Galileo GNSS - bool enable_galileo_; + bool enable_galileo_{false}; //! Whether or not to enable the BeiDuo GNSS - bool enable_beidou_; + bool enable_beidou_{false}; //! Whether or not to enable the IMES GNSS - bool enable_imes_; + bool enable_imes_{false}; //! Desired NMEA configuration. ublox_msgs::msg::CfgNMEA cfg_nmea_; //! Whether to clear the flash memory during configuration - bool clear_bbr_; - bool save_on_shutdown_; + bool clear_bbr_{false}; + bool save_on_shutdown_{false}; rclcpp::Publisher::SharedPtr nav_sat_pub_; rclcpp::Publisher::SharedPtr mon_hw_pub_; diff --git a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp index 3d508c84..ce181b8a 100644 --- a/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp +++ b/ublox_gps/include/ublox_gps/ublox_topic_diagnostic.hpp @@ -12,10 +12,15 @@ namespace ublox_node { //! Topic diagnostics for u-blox messages struct UbloxTopicDiagnostic { - UbloxTopicDiagnostic() {} + UbloxTopicDiagnostic() = default; // Must not copy this struct (would confuse FrequencyStatusParam pointers) - UbloxTopicDiagnostic(const UbloxTopicDiagnostic&) = delete; + UbloxTopicDiagnostic(UbloxTopicDiagnostic &&c) = delete; + UbloxTopicDiagnostic &operator=(UbloxTopicDiagnostic &&c) = delete; + UbloxTopicDiagnostic(const UbloxTopicDiagnostic &c) = delete; + UbloxTopicDiagnostic &operator=(const UbloxTopicDiagnostic &c) = delete; + + ~UbloxTopicDiagnostic() = default; /** * @brief Add a topic diagnostic to the diagnostic updater for @@ -61,9 +66,9 @@ struct UbloxTopicDiagnostic { //! Topic frequency diagnostic updater std::shared_ptr diagnostic; //! Minimum allow frequency of topic - double min_freq; + double min_freq{0.0}; //! Maximum allow frequency of topic - double max_freq; + double max_freq{0.0}; }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index ff4826a4..0e535b0d 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -1,14 +1,13 @@ #ifndef UBLOX_GPS_UTILS_HPP #define UBLOX_GPS_UTILS_HPP +#include #include #include #include #include #include -#include - #include #include @@ -20,7 +19,7 @@ namespace ublox_node { * @brief Convert date/time to UTC time in seconds. */ template -long toUtcSeconds(const NavPVT& msg) { +time_t toUtcSeconds(const NavPVT& msg) { // Create TM struct for mkgmtime struct tm time{}; time.tm_year = msg.year - 1900; @@ -131,7 +130,7 @@ void getRosUint(rclcpp::Node* node, const std::string& key, U &u, V default_val) */ template bool getRosUint(rclcpp::Node* node, const std::string& key, std::vector &u) { - std::vector param; + std::vector param; if (!node->get_parameter(key, param)) { return false; } @@ -158,7 +157,7 @@ static inline bool getRosBoolean(rclcpp::Node* node, const std::string &name) } template -T declareRosIntParameter(rclcpp::Node* node, const std::string& name, long int default_value) +T declareRosIntParameter(rclcpp::Node* node, const std::string& name, int64_t default_value) { rcl_interfaces::msg::ParameterDescriptor param_desc; param_desc.name = name; diff --git a/ublox_gps/include/ublox_gps/worker.hpp b/ublox_gps/include/ublox_gps/worker.hpp index 8af38976..93c5323c 100644 --- a/ublox_gps/include/ublox_gps/worker.hpp +++ b/ublox_gps/include/ublox_gps/worker.hpp @@ -37,11 +37,18 @@ namespace ublox_gps { /** * @brief Handles I/O reading and writing. */ -class Worker { +// clang-tidy insists that we follow rule-of-5 for this abstract base class. +// Generally we'd define the copy and move constructors as delete, but once +// we do that we also have to explicitly define a default constructor (otherwise +// it fails to compile). That all works, but harms the readability of this +// completely abstract base class (basically an interface), so we just disable +// the checks for this class. +class Worker { // NOLINT(hicpp-special-member-functions, cppcoreguidelines-special-member-functions) public: - typedef std::function WorkerCallback; - typedef std::function WorkerRawCallback; - virtual ~Worker() {} + using WorkerCallback = std::function; + using WorkerRawCallback = std::function; + + virtual ~Worker() = default; /** * @brief Set the callback function for received messages. diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 12f4fd40..1cbe8d6c 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -26,7 +26,7 @@ namespace ublox_node { // u-blox ADR devices, partially implemented // AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) - : nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) + : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { imu_pub_ = node_->create_publisher("imu_meas", 1); @@ -43,7 +43,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s void AdrUdrProduct::getRosParams() { use_adr_ = getRosBoolean(node_, "use_adr"); // Check the nav rate - float nav_rate_hz = 1000 / (meas_rate_ * nav_rate_); + float nav_rate_hz = 1000.0 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { RCLCPP_WARN(node_->get_logger(), "Nav Rate recommended to be 1 Hz"); } @@ -107,10 +107,10 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { float m_per_sec_sq = ::pow(2, -10); std::vector imu_data = m.data; - for (size_t i = 0; i < imu_data.size(); i++){ - unsigned int data_type = imu_data[i] >> 24; //grab the last six bits of data - double data_sign = (imu_data[i] & (1 << 23)); //grab the sign (+/-) of the rest of the data - unsigned int data_value = imu_data[i] & 0x7FFFFF; //grab the rest of the data...should be 23 bits + for (unsigned int datapoint : imu_data) { + unsigned int data_type = datapoint >> 24; //grab the last six bits of data + double data_sign = (datapoint & (1 << 23)); //grab the sign (+/-) of the rest of the data + unsigned int data_value = datapoint & 0x7FFFFF; //grab the rest of the data...should be 23 bits if (data_sign == 0) { data_sign = -1; diff --git a/ublox_gps/src/gnss.cpp b/ublox_gps/src/gnss.cpp index 35e15c73..c2155011 100644 --- a/ublox_gps/src/gnss.cpp +++ b/ublox_gps/src/gnss.cpp @@ -4,14 +4,6 @@ namespace ublox_node { -Gnss::Gnss() -{ -} - -Gnss::~Gnss() -{ -} - void Gnss::add(const std::string & gnss) { supported_.insert(gnss); diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index f451b7be..3d63f43d 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -49,7 +49,7 @@ const std::chrono::milliseconds Gps::default_timeout_ = std::chrono::milliseconds( static_cast(Gps::kDefaultAckTimeout * 1000)); -Gps::Gps(int debug, const rclcpp::Logger& logger) : configured_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug), logger_(logger) { +Gps::Gps(int debug, const rclcpp::Logger& logger) : configured_(false), save_on_shutdown_(false), config_on_startup_flag_(true), debug_(debug), callbacks_(debug), logger_(logger) { subscribeAcks(); } @@ -84,7 +84,7 @@ void Gps::subscribeAcks() { void Gps::processAck(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages - Ack ack; + Ack ack{}; ack.type = ACK; ack.class_id = m.cls_id; ack.msg_id = m.msg_id; @@ -96,7 +96,7 @@ void Gps::processAck(const ublox_msgs::msg::Ack &m) { void Gps::processNack(const ublox_msgs::msg::Ack &m) { // Process ACK/NACK messages - Ack ack; + Ack ack{}; ack.type = NACK; ack.class_id = m.cls_id; ack.msg_id = m.msg_id; @@ -107,10 +107,10 @@ void Gps::processNack(const ublox_msgs::msg::Ack &m) { void Gps::processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m) { if (m.cmd == ublox_msgs::msg::UpdSOSAck::CMD_BACKUP_CREATE_ACK) { - Ack ack; - ack.type = (m.response == m.BACKUP_CREATE_ACK) ? ACK : NACK; - ack.class_id = m.CLASS_ID; - ack.msg_id = m.MESSAGE_ID; + Ack ack{}; + ack.type = (m.response == ublox_msgs::msg::UpdSOSAck::BACKUP_CREATE_ACK) ? ACK : NACK; + ack.class_id = ublox_msgs::msg::UpdSOSAck::CLASS_ID; + ack.msg_id = ublox_msgs::msg::UpdSOSAck::MESSAGE_ID; // store the ack atomically ack_.store(ack, std::memory_order_seq_cst); RCLCPP_DEBUG_EXPRESSION(logger_, ack.type == ACK && debug_ >= 2, @@ -138,7 +138,7 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, RCLCPP_INFO(logger_, "U-Blox: Opened serial port %s", port.c_str()); int fd = serial->native_handle(); - termios tio; + termios tio{}; tcgetattr(fd, &tio); cfmakeraw(&tio); tcsetattr(fd, TCSANOW, &tio); @@ -155,15 +155,15 @@ void Gps::initializeSerial(const std::string & port, unsigned int baudrate, asio::serial_port_base::baud_rate current_baudrate; serial->get_option(current_baudrate); // Incrementally increase the baudrate to the desired value - for (size_t i = 0; i < sizeof(kBaudrates) / sizeof(kBaudrates[0]); i++) { + for (unsigned int fixed_baudrate : kBaudrates) { if (current_baudrate.value() == baudrate) { break; } // Don't step down, unless the desired baudrate is lower - if (current_baudrate.value() > kBaudrates[i] && baudrate > kBaudrates[i]) { + if (current_baudrate.value() > fixed_baudrate && baudrate > fixed_baudrate) { continue; } - serial->set_option(asio::serial_port_base::baud_rate(kBaudrates[i])); + serial->set_option(asio::serial_port_base::baud_rate(fixed_baudrate)); std::this_thread::sleep_for( std::chrono::milliseconds(kSetBaudrateSleepMs)); serial->get_option(current_baudrate); @@ -286,10 +286,7 @@ bool Gps::configReset(uint16_t nav_bbr_mask, uint16_t reset_mode) { rst.reset_mode = reset_mode; // Don't wait for ACK, return if it fails - if (!configure(rst, false)) { - return false; - } - return true; + return configure(rst, false); } bool Gps::configGnss(ublox_msgs::msg::CfgGNSS gnss, @@ -313,8 +310,8 @@ bool Gps::configGnss(ublox_msgs::msg::CfgGNSS gnss, bool Gps::saveOnShutdown() { // Command the receiver to stop ublox_msgs::msg::CfgRST rst; - rst.nav_bbr_mask = rst.NAV_BBR_HOT_START; - rst.reset_mode = rst.RESET_MODE_GNSS_STOP; + rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_HOT_START; + rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_GNSS_STOP; if (!configure(rst)) { return false; } @@ -328,7 +325,7 @@ bool Gps::clearBbr() { // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK ublox_msgs::msg::UpdSOS sos; - sos.cmd = sos.CMD_FLASH_BACKUP_CLEAR; + sos.cmd = ublox_msgs::msg::UpdSOS::CMD_FLASH_BACKUP_CLEAR; return configure(sos); } @@ -407,10 +404,10 @@ bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) { } bool Gps::configRtcm(const std::vector & rtcms) { - for (size_t i = 0; i < rtcms.size(); ++i) { - RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcms[i].id, rtcms[i].rate); - if (!setRate(ublox_msgs::Class::RTCM, rtcms[i].id, rtcms[i].rate)) { - RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcms[i].id, rtcms[i].rate); + for (const Rtcm & rtcm : rtcms) { + RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcm.id, rtcm.rate); + if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate)) { + RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcm.id, rtcm.rate); return false; } } @@ -440,26 +437,26 @@ bool Gps::configTmode3Fixed(bool lla_flag, RCLCPP_DEBUG(logger_, "Configuring TMODE3 to Fixed"); ublox_msgs::msg::CfgTMODE3 tmode3; - tmode3.flags = tmode3.FLAGS_MODE_FIXED & tmode3.FLAGS_MODE_MASK; - tmode3.flags |= lla_flag ? tmode3.FLAGS_LLA : 0; + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_FIXED & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; + tmode3.flags |= lla_flag ? ublox_msgs::msg::CfgTMODE3::FLAGS_LLA : 0; // Set position if (lla_flag) { // Convert from [deg] to [deg * 1e-7] - tmode3.ecef_x_or_lat = (int)round(arp_position[0] * 1e7); - tmode3.ecef_y_or_lon = (int)round(arp_position[1] * 1e7); - tmode3.ecef_z_or_alt = (int)round(arp_position[2] * 1e7); + tmode3.ecef_x_or_lat = static_cast(round(arp_position[0] * 1e7)); + tmode3.ecef_y_or_lon = static_cast(round(arp_position[1] * 1e7)); + tmode3.ecef_z_or_alt = static_cast(round(arp_position[2] * 1e7)); } else { // Convert from m to cm - tmode3.ecef_x_or_lat = (int)round(arp_position[0] * 1e2); - tmode3.ecef_y_or_lon = (int)round(arp_position[1] * 1e2); - tmode3.ecef_z_or_alt = (int)round(arp_position[2] * 1e2); + tmode3.ecef_x_or_lat = static_cast(round(arp_position[0] * 1e2)); + tmode3.ecef_y_or_lon = static_cast(round(arp_position[1] * 1e2)); + tmode3.ecef_z_or_alt = static_cast(round(arp_position[2] * 1e2)); } tmode3.ecef_x_or_lat_hp = arp_position_hp[0]; tmode3.ecef_y_or_lon_hp = arp_position_hp[1]; tmode3.ecef_z_or_alt_hp = arp_position_hp[2]; // Convert from m to [0.1 mm] - tmode3.fixed_pos_acc = (uint32_t)round(fixed_pos_acc * 1e4); + tmode3.fixed_pos_acc = static_cast(round(fixed_pos_acc * 1e4)); return configure(tmode3); } @@ -467,10 +464,10 @@ bool Gps::configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit) { ublox_msgs::msg::CfgTMODE3 tmode3; RCLCPP_DEBUG(logger_, "Setting TMODE3 to Survey In"); - tmode3.flags = tmode3.FLAGS_MODE_SURVEY_IN & tmode3.FLAGS_MODE_MASK; + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_SURVEY_IN & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; tmode3.svin_min_dur = svin_min_dur; // Convert from m to [0.1 mm] - tmode3.svin_acc_limit = (int)round(svin_acc_limit * 1e4); + tmode3.svin_acc_limit = static_cast(round(svin_acc_limit * 1e4)); return configure(tmode3); } @@ -478,7 +475,7 @@ bool Gps::disableTmode3() { RCLCPP_DEBUG(logger_, "Disabling TMODE3"); ublox_msgs::msg::CfgTMODE3 tmode3; - tmode3.flags = tmode3.FLAGS_MODE_DISABLED & tmode3.FLAGS_MODE_MASK; + tmode3.flags = ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_DISABLED & ublox_msgs::msg::CfgTMODE3::FLAGS_MODE_MASK; return configure(tmode3); } diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index c971a3da..d632597f 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -22,27 +22,26 @@ namespace ublox_node { // HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) - : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) + : tmode3_(0), lla_flag_(false), fixed_pos_acc_(0.0), svin_reset_(false), sv_in_min_dur_(0), sv_in_acc_lim_(0.0), nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) { navsvin_pub_ = node_->create_publisher("navsvin", 1); } /** - * @brief Get a int (size 8 or 16) vector from the parameter server. + * @brief Get a int (size 8) vector from the parameter server. * @throws std::runtime_error if the parameter is out of bounds. * @return true if found, false if not found. */ -template -bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { - std::vector param; +bool getRosInt(rclcpp::Node* node, const std::string& key, std::vector &i) { + std::vector param; if (!node->get_parameter(key, param)) { return false; } // Check the bounds - I min = std::numeric_limits::lowest(); - I max = std::numeric_limits::max(); + int8_t min = std::numeric_limits::lowest(); + int8_t max = std::numeric_limits::max(); checkRange(param, min, max, key); // set the output @@ -134,8 +133,8 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { " configuring survey-in"); } // Don't reset survey in if in time mode with a good fix - if (nav_pvt.fix_type == nav_pvt.FIX_TYPE_TIME_ONLY - && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) { + if (nav_pvt.fix_type == ublox_msgs::msg::NavPVT::FIX_TYPE_TIME_ONLY + && nav_pvt.flags & ublox_msgs::msg::NavPVT::FLAGS_GNSS_FIX_OK) { setTimeMode(gps); return true; } @@ -148,7 +147,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { meas_rate_temp = kDefaultMeasPeriod; } // Set nav rate to 1 Hz during survey in - if (!gps->configRate(meas_rate_temp, (int) 1000 / meas_rate_temp)) { + if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp)) { throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") + "before setting TMODE3 to survey-in."); } diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp index 865c6f39..09f19d39 100644 --- a/ublox_gps/src/hpg_rov_product.cpp +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -21,14 +21,14 @@ namespace ublox_node { // U-Blox High Precision GNSS Rover // HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node) - : nav_rate_(nav_rate), updater_(updater), node_(node) + : dgnss_mode_(ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED), nav_rate_(nav_rate), updater_(updater), node_(node) { nav_rel_pos_ned_pub_ = node_->create_publisher("navrelposned", 1); } void HpgRovProduct::getRosParams() { - // default to float, see CfgDGNSS message for details + // default to fixed, see CfgDGNSS message for details getRosUint(node_, "dgnss_mode", dgnss_mode_, ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED); } @@ -48,9 +48,9 @@ void HpgRovProduct::subscribe(std::shared_ptr gps) { } void HpgRovProduct::initializeRosDiagnostics() { - freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"), - kRtcmFreqMin, kRtcmFreqMax, - kRtcmFreqTol, kRtcmFreqWindow, updater_); + freq_rtcm_ = std::make_unique(std::string("rxmrtcm"), + kRtcmFreqMin, kRtcmFreqMax, + kRtcmFreqTol, kRtcmFreqWindow, updater_); updater_->add("Carrier Phase Solution", this, &HpgRovProduct::carrierPhaseDiagnostics); updater_->force_update(); @@ -58,18 +58,18 @@ void HpgRovProduct::initializeRosDiagnostics() { void HpgRovProduct::carrierPhaseDiagnostics( diagnostic_updater::DiagnosticStatusWrapper& stat) { - uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK; + uint32_t carr_soln = last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_MASK; stat.add("iTow", last_rel_pos_.i_tow); - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE || - !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN && - last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) { + if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_NONE || + !(last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_DIFF_SOLN && + last_rel_pos_.flags & ublox_msgs::msg::NavRELPOSNED::FLAGS_REL_POS_VALID)) { stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat.message = "None"; } else { - if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) { + if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_FLOAT) { stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat.message = "Float"; - } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) { + } else if (carr_soln & ublox_msgs::msg::NavRELPOSNED::FLAGS_CARR_SOLN_FIXED) { stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat.message = "Fixed"; } diff --git a/ublox_gps/src/mkgmtime.c b/ublox_gps/src/mkgmtime.c index e80d28ef..47e61f8f 100644 --- a/ublox_gps/src/mkgmtime.c +++ b/ublox_gps/src/mkgmtime.c @@ -87,70 +87,80 @@ */ #include "ublox_gps/mkgmtime.h" -#ifndef WRONG -#define WRONG (-1) -#endif /* !defined WRONG */ -static int tmcomp(register const struct tm * const atmp, - register const struct tm * const btmp) +static int tmcomp(const struct tm * const atmp, + const struct tm * const btmp) { - register int result; + int result; - if ((result = (atmp->tm_year - btmp->tm_year)) == 0 && - (result = (atmp->tm_mon - btmp->tm_mon)) == 0 && - (result = (atmp->tm_mday - btmp->tm_mday)) == 0 && - (result = (atmp->tm_hour - btmp->tm_hour)) == 0 && - (result = (atmp->tm_min - btmp->tm_min)) == 0) - result = atmp->tm_sec - btmp->tm_sec; - return result; + if ((result = (atmp->tm_year - btmp->tm_year)) == 0 && + (result = (atmp->tm_mon - btmp->tm_mon)) == 0 && + (result = (atmp->tm_mday - btmp->tm_mday)) == 0 && + (result = (atmp->tm_hour - btmp->tm_hour)) == 0 && + (result = (atmp->tm_min - btmp->tm_min)) == 0) { + result = atmp->tm_sec - btmp->tm_sec; + } + return result; } -time_t mkgmtime(struct tm * const tmp) { - register int dir; - register int bits; - register int saved_seconds; - time_t t; - struct tm yourtm, *mytm; +time_t mkgmtime(struct tm * const tmp) +{ + int dir; + int bits; + int saved_seconds; + time_t t; + struct tm yourtm, *mytm; + + yourtm = *tmp; + saved_seconds = yourtm.tm_sec; + yourtm.tm_sec = 0; + /* + * Calculate the number of magnitude bits in a time_t + * (this works regardless of whether time_t is + * signed or unsigned, though lint complains if unsigned). + */ + for (bits = 0, t = 1; t > 0; ++bits, t <<= 1) + { + } - yourtm = *tmp; - saved_seconds = yourtm.tm_sec; - yourtm.tm_sec = 0; - /* - ** Calculate the number of magnitude bits in a time_t - ** (this works regardless of whether time_t is - ** signed or unsigned, though lint complains if unsigned). - */ - for (bits = 0, t = 1; t > 0; ++bits, t <<= 1) - ; - /* - ** If time_t is signed, then 0 is the median value, - ** if time_t is unsigned, then 1 << bits is median. - */ - t = (t < 0) ? 0 : ((time_t) 1 << bits); + /* + * If time_t is signed, then 0 is the median value, + * if time_t is unsigned, then 1 << bits is median. + */ + t = (t < 0) ? 0 : ((time_t) 1 << bits); - /* Some gmtime() implementations are broken and will return - * NULL for time_ts larger than 40 bits even on 64-bit platforms - * so we'll just cap it at 40 bits */ - if(bits > 40) bits = 40; + /* Some gmtime() implementations are broken and will return + * NULL for time_ts larger than 40 bits even on 64-bit platforms + * so we'll just cap it at 40 bits */ + if (bits > 40) { + bits = 40; + } - for ( ; ; ) { - mytm = gmtime(&t); + for ( ; ; ) { + mytm = gmtime(&t); - if(!mytm) return WRONG; + if (!mytm) { + return -1; + } - dir = tmcomp(mytm, &yourtm); - if (dir != 0) { - if (bits-- < 0) - return WRONG; - if (bits < 0) - --t; - else if (dir > 0) - t -= (time_t) 1 << bits; - else t += (time_t) 1 << bits; - continue; - } - break; + dir = tmcomp(mytm, &yourtm); + if (dir != 0) { + if (bits-- < 0) { + return -1; + } + if (bits < 0) { + --t; + } + else if (dir > 0) { + t -= (time_t) 1 << bits; + } + else { + t += (time_t) 1 << bits; + } + continue; } - t += saved_seconds; - return t; + break; + } + t += saved_seconds; + return t; } diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index a94fb1fa..5df714fa 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -88,21 +89,29 @@ uint8_t modelFromString(const std::string& model) { std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "portable") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PORTABLE; - } else if (lower == "stationary") { + } + if (lower == "stationary") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_STATIONARY; - } else if (lower == "pedestrian") { + } + if (lower == "pedestrian") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_PEDESTRIAN; - } else if (lower == "automotive") { + } + if (lower == "automotive") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AUTOMOTIVE; - } else if (lower == "sea") { + } + if (lower == "sea") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_SEA; - } else if (lower == "airborne1") { + } + if (lower == "airborne1") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_1G; - } else if (lower == "airborne2") { + } + if (lower == "airborne2") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_2G; - } else if (lower == "airborne4") { + } + if (lower == "airborne4") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_AIRBORNE_4G; - } else if (lower == "wristwatch") { + } + if (lower == "wristwatch") { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_WRIST_WATCH; } @@ -124,9 +133,11 @@ uint8_t fixModeFromString(const std::string& mode) { std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { return ublox_msgs::msg::CfgNAV5::FIX_MODE_2D_ONLY; - } else if (lower == "3d") { + } + if (lower == "3d") { return ublox_msgs::msg::CfgNAV5::FIX_MODE_3D_ONLY; - } else if (lower == "auto") { + } + if (lower == "auto") { return ublox_msgs::msg::CfgNAV5::FIX_MODE_AUTO; } @@ -185,13 +196,13 @@ UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { void UbloxNode::addFirmwareInterface() { int ublox_version; - if (protocol_version_ < 14) { + if (protocol_version_ < 14.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 6; - } else if (protocol_version_ >= 14 && protocol_version_ <= 15) { + } else if (protocol_version_ >= 14.0 && protocol_version_ <= 15.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 7; - } else if (protocol_version_ > 15 && protocol_version_ <= 23) { + } else if (protocol_version_ > 15.0 && protocol_version_ <= 23.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 8; } else { @@ -205,20 +216,20 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { - if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0) { + if (product_category == "HPG" && ref_rov == "REF") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); - } else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0) { + } else if (product_category == "HPG" && ref_rov == "ROV") { components_.push_back(std::make_shared(nav_rate_, updater_, this)); - } else if (product_category.compare("HPG") == 0) { + } else if (product_category == "HPG") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); - } else if (product_category.compare("TIM") == 0) { + } else if (product_category == "TIM") { components_.push_back(std::make_shared(frame_id_, updater_, this)); - } else if (product_category.compare("ADR") == 0 || - product_category.compare("UDR") == 0) { + } else if (product_category == "ADR" || + product_category == "UDR") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); - } else if (product_category.compare("FTS") == 0) { + } else if (product_category == "FTS") { components_.push_back(std::make_shared()); - } else if (product_category.compare("SPG") != 0) { + } else if (product_category == "SPG") { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); @@ -542,14 +553,14 @@ void UbloxNode::subscribe() { 1); } - for (size_t i = 0; i < components_.size(); i++) { - components_[i]->subscribe(gps_); + for (const std::shared_ptr & component : components_) { + component->subscribe(gps_); } } void UbloxNode::initializeRosDiagnostics() { - for (size_t i = 0; i < components_.size(); i++) { - components_[i]->initializeRosDiagnostics(); + for (const std::shared_ptr & component : components_) { + component->initializeRosDiagnostics(); } } @@ -563,49 +574,55 @@ void UbloxNode::processMonVer() { std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(), std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str()); // Convert extension to vector of strings - std::vector extension; - extension.reserve(monVer.extension.size()); - for (std::size_t i = 0; i < monVer.extension.size(); ++i) { + std::vector extensions; + extensions.reserve(monVer.extension.size()); + for (std::size_t i = 0; i < monVer.extension.size(); ++i) { // NOLINT(modernize-loop-convert) RCLCPP_DEBUG(this->get_logger(), "%s", std::string(monVer.extension[i].field.begin(), monVer.extension[i].field.end()).c_str()); // Find the end of the string (null character) unsigned char* end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); - extension.push_back(std::string(monVer.extension[i].field.begin(), end)); + extensions.emplace_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for (std::size_t i = 0; i < extension.size(); ++i) { - std::size_t found = extension[i].find("PROTVER"); + for (const std::string & ext : extensions) { + std::size_t found = ext.find("PROTVER"); if (found != std::string::npos) { - protocol_version_ = ::atof( - extension[i].substr(8, extension[i].size()-8).c_str()); + const char * sub = ext.substr(8, ext.size()-8).c_str(); + char * end{nullptr}; + protocol_version_ = std::strtof(sub, &end); + if (protocol_version_ == HUGE_VALF || (protocol_version_ == 0 && end == sub)) { + // strtof failed to convert either via overflow or no conversion possible. + // Throw an error. + throw std::runtime_error("Failed to parse protocol version from extensions"); + } break; } } - if (protocol_version_ == 0) { + if (protocol_version_ == 0.0) { RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); } addFirmwareInterface(); - if (protocol_version_ < 18) { + if (protocol_version_ < 18.0) { // Final line contains supported GNSS delimited by ; std::vector strs; - if (extension.size() > 0) { - strs = stringSplit(extension[extension.size() - 1], ";"); + if (extensions.size() > 0) { + strs = stringSplit(extensions[extensions.size() - 1], ";"); } - for (size_t i = 0; i < strs.size(); i++) { - gnss_->add(strs[i]); + for (const std::string & str : strs) { + gnss_->add(str); } } else { - for (std::size_t i = 0; i < extension.size(); ++i) { + for (std::size_t i = 0; i < extensions.size(); ++i) { std::vector strs; // Up to 2nd to last line - if (i <= extension.size() - 2) { - strs = stringSplit(extension[i], "="); + if (i <= extensions.size() - 2) { + strs = stringSplit(extensions[i], "="); if (strs.size() > 1) { - if (strs[0].compare(std::string("FWVER")) == 0) { + if (strs[0] == "FWVER") { if (strs[1].length() > 8) { addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); } else { @@ -616,10 +633,10 @@ void UbloxNode::processMonVer() { } } // Last 1-2 lines contain supported GNSS - if (i >= extension.size() - 2) { - strs = stringSplit(extension[i], ";"); - for (size_t i = 0; i < strs.size(); i++) { - gnss_->add(strs[i]); + if (i >= extensions.size() - 2) { + strs = stringSplit(extensions[i], ";"); + for (const std::string & str : strs) { + gnss_->add(str); } } } @@ -637,14 +654,15 @@ bool UbloxNode::configureUblox() { throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); } - if (load_.load_mask & load_.MASK_IO_PORT) { + if (load_.load_mask & ublox_msgs::msg::CfgCFG::MASK_IO_PORT) { RCLCPP_DEBUG(this->get_logger(), "Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); gps_->reset(wait); - if (!gps_->isConfigured()) + if (!gps_->isConfigured()) { throw std::runtime_error(std::string("Failed to reset serial I/O") + "after loading I/O configurations from device memory."); + } } } @@ -686,15 +704,15 @@ bool UbloxNode::configureUblox() { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component - for (size_t i = 0; i < components_.size(); i++) { - if (!components_[i]->configureUblox(gps_)) { + for (const std::shared_ptr & component : components_) { + if (!component->configureUblox(gps_)) { return false; } } } if (save_.save_mask != 0) { RCLCPP_DEBUG(this->get_logger(), "Saving the u-blox configuration, mask %u, device %u", - save_.save_mask, save_.device_mask); + save_.save_mask, save_.device_mask); if (!gps_->configure(save_)) { RCLCPP_ERROR(this->get_logger(), "u-blox unable to save configuration to non-volatile memory"); } @@ -710,14 +728,14 @@ void UbloxNode::configureInf() { ublox_msgs::msg::CfgINF msg; // Subscribe to UBX INF messages ublox_msgs::msg::CfgINFBlock block; - block.protocol_id = block.PROTOCOL_ID_UBX; + block.protocol_id = ublox_msgs::msg::CfgINFBlock::PROTOCOL_ID_UBX; // Enable desired INF messages on each UBX port - uint8_t mask = (getRosBoolean(this, "inf.error") ? block.INF_MSG_ERROR : 0) | - (getRosBoolean(this, "inf.warning") ? block.INF_MSG_WARNING : 0) | - (getRosBoolean(this, "inf.notice") ? block.INF_MSG_NOTICE : 0) | - (getRosBoolean(this, "inf.test") ? block.INF_MSG_TEST : 0) | - (getRosBoolean(this, "inf.debug") ? block.INF_MSG_DEBUG : 0); - for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { + uint8_t mask = (getRosBoolean(this, "inf.error") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_ERROR : 0) | + (getRosBoolean(this, "inf.warning") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_WARNING : 0) | + (getRosBoolean(this, "inf.notice") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_NOTICE : 0) | + (getRosBoolean(this, "inf.test") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_TEST : 0) | + (getRosBoolean(this, "inf.debug") ? ublox_msgs::msg::CfgINFBlock::INF_MSG_DEBUG : 0); + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { // NOLINT(modernize-loop-convert) block.inf_msg_mask[i] = mask; } @@ -726,9 +744,9 @@ void UbloxNode::configureInf() { // IF NMEA is enabled if (uart_in_ & ublox_msgs::msg::CfgPRT::PROTO_NMEA) { ublox_msgs::msg::CfgINFBlock block; - block.protocol_id = block.PROTOCOL_ID_NMEA; + block.protocol_id = ublox_msgs::msg::CfgINFBlock::PROTOCOL_ID_NMEA; // Enable desired INF messages on each NMEA port - for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { + for (size_t i = 0; i < block.inf_msg_mask.size(); i++) { // NOLINT(modernize-loop-convert) block.inf_msg_mask[i] = mask; } msg.blocks.push_back(block); @@ -780,14 +798,14 @@ void UbloxNode::initialize() { initializeIo(); // Must process Mon VER before setting firmware/hardware params processMonVer(); - if (protocol_version_ <= 14) { + if (protocol_version_ <= 14.0) { if (getRosBoolean(this, "raw_data")) { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, this)); } } // Must set firmware & hardware params before initializing diagnostics - for (size_t i = 0; i < components_.size(); i++) { - components_[i]->getRosParams(); + for (const std::shared_ptr & component : components_) { + component->getRosParams(); } // Do this last initializeRosDiagnostics(); diff --git a/ublox_gps/src/raw_data_pa.cpp b/ublox_gps/src/raw_data_pa.cpp index 08b54b85..dedd7bb9 100644 --- a/ublox_gps/src/raw_data_pa.cpp +++ b/ublox_gps/src/raw_data_pa.cpp @@ -34,12 +34,12 @@ #include #include +#include #include #include #include #include -#include #include @@ -79,9 +79,9 @@ bool RawDataStreamPa::isEnabled() { if (is_ros_subscriber_) { return !file_dir_.empty(); - } else { - return flag_publish_ || (!file_dir_.empty()); } + + return flag_publish_ || !file_dir_.empty(); } void RawDataStreamPa::initialize() { @@ -97,7 +97,7 @@ void RawDataStreamPa::initialize() { } if (!file_dir_.empty()) { - struct stat stat_info; + struct stat stat_info{}; if (::stat(file_dir_.c_str(), &stat_info) != 0) { RCLCPP_ERROR(this->get_logger(), "Can't log raw data to file. " "Directory \"%s\" does not exist.", file_dir_.c_str()); @@ -111,7 +111,7 @@ void RawDataStreamPa::initialize() { file_dir_ += '/'; } - time_t t = time(NULL); + time_t t = time(nullptr); struct tm time_struct = *localtime(&t); std::stringstream filename; @@ -146,7 +146,7 @@ void RawDataStreamPa::initialize() { void RawDataStreamPa::ubloxCallback(const unsigned char* data, std::size_t size) { - std::string str((const char*) data, size); + std::string str(reinterpret_cast(data), size); if (flag_publish_) { publishMsg(str); diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index 4ba33454..82f556c3 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -67,16 +67,16 @@ void UbloxFirmware6::getRosParams() { } // set flags - cfg_nmea_.flags = getRosBoolean(node_, "nmea.compat") ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea.consider") ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = getRosBoolean(node_, "nmea.compat") ? ublox_msgs::msg::CfgNMEA6::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.consider") ? ublox_msgs::msg::CfgNMEA6::FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.sbas") ? cfg_nmea_.FILTER_SBAS_FILT : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA6::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA6::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA6::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA6::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.sbas") ? ublox_msgs::msg::CfgNMEA6::FILTER_SBAS_FILT : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA6::FILTER_TRACK : 0; } } @@ -154,7 +154,7 @@ void UbloxFirmware6::fixDiagnostic( stat.add("Height above MSL [m]", last_nav_pos_.h_msl * 1e-3); stat.add("Horizontal Accuracy [m]", last_nav_pos_.h_acc * 1e-3); stat.add("Vertical Accuracy [m]", last_nav_pos_.v_acc * 1e-3); - stat.add("# SVs used", (int)last_nav_sol_.num_sv); + stat.add("# SVs used", static_cast(last_nav_sol_.num_sv)); } void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { @@ -174,10 +174,10 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { fix_.longitude = m.lon * 1e-7; fix_.altitude = m.height * 1e-3; - if (last_nav_sol_.gps_fix >= last_nav_sol_.GPS_2D_FIX) { - fix_.status.status = fix_.status.STATUS_FIX; + if (last_nav_sol_.gps_fix >= ublox_msgs::msg::NavSOL::GPS_2D_FIX) { + fix_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; } else { - fix_.status.status = fix_.status.STATUS_NO_FIX; + fix_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; } // Convert from mm to m @@ -190,7 +190,7 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { fix_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - fix_.status.service = fix_.status.SERVICE_GPS; + fix_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; fix_pub_->publish(fix_); last_nav_pos_ = m; // update diagnostics diff --git a/ublox_gps/src/ublox_firmware7.cpp b/ublox_gps/src/ublox_firmware7.cpp index 00ecfc84..03dc009c 100644 --- a/ublox_gps/src/ublox_firmware7.cpp +++ b/ublox_gps/src/ublox_firmware7.cpp @@ -87,20 +87,20 @@ void UbloxFirmware7::getRosParams() { } // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; + cfg_nmea_.flags = compat ? ublox_msgs::msg::CfgNMEA7::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? ublox_msgs::msg::CfgNMEA7::FLAGS_CONSIDER : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA7::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA7::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA7::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA7::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? ublox_msgs::msg::CfgNMEA7::FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA7::FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? ublox_msgs::msg::CfgNMEA7::GNSS_TO_FILTER_GLONASS : 0; getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); @@ -127,10 +127,10 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { // configure GLONASS if (gnss_->isSupported("GLO")) { ublox_msgs::msg::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_GLONASS; - block.res_trk_ch = block.RES_TRK_CH_GLONASS; - block.max_trk_ch = block.MAX_TRK_CH_GLONASS; - block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GLONASS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_GLONASS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_GLONASS; + block.flags = enable_glonass_ ? ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_GLONASS_L1OF : 0; cfgGNSSWrite.blocks.push_back(block); if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + @@ -142,9 +142,9 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { if (gnss_->isSupported("QZSS")) { // configure QZSS ublox_msgs::msg::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_QZSS; - block.res_trk_ch = block.RES_TRK_CH_QZSS; - block.max_trk_ch = block.MAX_TRK_CH_QZSS; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_QZSS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_QZSS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_QZSS; block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { @@ -157,10 +157,10 @@ bool UbloxFirmware7::configureUblox(std::shared_ptr gps) { if (gnss_->isSupported("SBAS")) { // configure SBAS ublox_msgs::msg::CfgGNSSBlock block; - block.gnss_id = block.GNSS_ID_SBAS; - block.res_trk_ch = block.RES_TRK_CH_SBAS; - block.max_trk_ch = block.MAX_TRK_CH_SBAS; - block.flags = getRosBoolean(node_, "gnss.sbas") ? block.SIG_CFG_SBAS_L1CA : 0; + block.gnss_id = ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_SBAS; + block.res_trk_ch = ublox_msgs::msg::CfgGNSSBlock::RES_TRK_CH_SBAS; + block.max_trk_ch = ublox_msgs::msg::CfgGNSSBlock::MAX_TRK_CH_SBAS; + block.flags = getRosBoolean(node_, "gnss.sbas") ? ublox_msgs::msg::CfgGNSSBlock::SIG_CFG_SBAS_L1CA : 0; cfgGNSSWrite.blocks[0] = block; if (!gps->configure(cfgGNSSWrite)) { throw std::runtime_error(std::string("Failed to ") + diff --git a/ublox_gps/src/ublox_firmware8.cpp b/ublox_gps/src/ublox_firmware8.cpp index e1b80759..eb7e3ba7 100644 --- a/ublox_gps/src/ublox_firmware8.cpp +++ b/ublox_gps/src/ublox_firmware8.cpp @@ -76,7 +76,7 @@ void UbloxFirmware8::getRosParams() { // if (getRosBoolean(node_, "nmea.set")) { bool compat, consider; - cfg_nmea_.version = cfg_nmea_.VERSION; // message version + cfg_nmea_.version = ublox_msgs::msg::CfgNMEA::VERSION; // message version // Verify that parameters are set if (!getRosUint(node_, "nmea.version", cfg_nmea_.nmea_version)) { @@ -101,23 +101,23 @@ void UbloxFirmware8::getRosParams() { } // set flags - cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0; - cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea.limit82") ? cfg_nmea_.FLAGS_LIMIT82 : 0; - cfg_nmea_.flags |= getRosBoolean(node_, "nmea.high_prec") ? cfg_nmea_.FLAGS_HIGH_PREC : 0; + cfg_nmea_.flags = compat ? ublox_msgs::msg::CfgNMEA::FLAGS_COMPAT : 0; + cfg_nmea_.flags |= consider ? ublox_msgs::msg::CfgNMEA::FLAGS_CONSIDER : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.limit82") ? ublox_msgs::msg::CfgNMEA::FLAGS_LIMIT82 : 0; + cfg_nmea_.flags |= getRosBoolean(node_, "nmea.high_prec") ? ublox_msgs::msg::CfgNMEA::FLAGS_HIGH_PREC : 0; // set filter - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? cfg_nmea_.FILTER_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? cfg_nmea_.FILTER_MSK_POS : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? cfg_nmea_.FILTER_TIME : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? cfg_nmea_.FILTER_DATE : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? cfg_nmea_.FILTER_GPS_ONLY : 0; - cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? cfg_nmea_.FILTER_TRACK : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.pos") ? ublox_msgs::msg::CfgNMEA::FILTER_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.msk_pos") ? ublox_msgs::msg::CfgNMEA::FILTER_MSK_POS : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.time") ? ublox_msgs::msg::CfgNMEA::FILTER_TIME : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.date") ? ublox_msgs::msg::CfgNMEA::FILTER_DATE : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.gps_only") ? ublox_msgs::msg::CfgNMEA::FILTER_GPS_ONLY : 0; + cfg_nmea_.filter |= getRosBoolean(node_, "nmea.filter.track") ? ublox_msgs::msg::CfgNMEA::FILTER_TRACK : 0; // set gnssToFilter - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0; - cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.beidou") ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.gps") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_GPS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.sbas") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_SBAS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.qzss") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_QZSS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.glonass") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_GLONASS : 0; + cfg_nmea_.gnss_to_filter |= getRosBoolean(node_, "nmea.gnssToFilter.beidou") ? ublox_msgs::msg::CfgNMEA::GNSS_TO_FILTER_BEIDOU : 0; getRosUint(node_, "nmea.main_talker_id", cfg_nmea_.main_talker_id); getRosUint(node_, "nmea.gsv_talker_id", cfg_nmea_.gsv_talker_id); @@ -154,58 +154,58 @@ bool UbloxFirmware8::configureUblox(std::shared_ptr gps) { // Then, check the configuration for each GNSS. If it is different, change it. bool correct = true; - for (size_t i = 0; i < cfg_gnss.blocks.size(); i++) { + for (size_t i = 0; i < cfg_gnss.blocks.size(); i++) { // NOLINT(modernize-loop-convert) ublox_msgs::msg::CfgGNSSBlock block = cfg_gnss.blocks[i]; - if (block.gnss_id == block.GNSS_ID_GPS - && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) { + if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GPS + && enable_gps_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_; + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_gps_; RCLCPP_DEBUG(node_->get_logger(), "GPS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_SBAS - && getRosBoolean(node_, "gnss.sbas") != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_SBAS + && getRosBoolean(node_, "gnss.sbas") != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | getRosBoolean(node_, "gnss.sbas"); + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | getRosBoolean(node_, "gnss.sbas"); RCLCPP_DEBUG(node_->get_logger(), "SBAS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_GALILEO - && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GALILEO + && enable_galileo_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_; + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_galileo_; RCLCPP_DEBUG(node_->get_logger(), "Galileo GNSS Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_BEIDOU - && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_BEIDOU + && enable_beidou_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_; + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_beidou_; RCLCPP_DEBUG(node_->get_logger(), "BeiDou Configuration is different"); - } else if (block.gnss_id == block.GNSS_ID_IMES - && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_IMES + && enable_imes_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_; - } else if (block.gnss_id == block.GNSS_ID_QZSS - && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE) + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_imes_; + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_QZSS + && (enable_qzss_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) || (enable_qzss_ - && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) { + && qzss_sig_cfg_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_SIG_CFG_MASK)))) { RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration is different %u, %u", - block.flags & block.FLAGS_ENABLE, + block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE, enable_qzss_); correct = false; RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", block.flags); cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_; + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_qzss_; RCLCPP_DEBUG(node_->get_logger(), "QZSS Configuration: %u", cfg_gnss.blocks[i].flags); if (enable_qzss_) { // Only change sig cfg if enabling cfg_gnss.blocks[i].flags |= qzss_sig_cfg_; } - } else if (block.gnss_id == block.GNSS_ID_GLONASS - && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) { + } else if (block.gnss_id == ublox_msgs::msg::CfgGNSSBlock::GNSS_ID_GLONASS + && enable_glonass_ != (block.flags & ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE)) { correct = false; cfg_gnss.blocks[i].flags = - (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_; + (cfg_gnss.blocks[i].flags & ~ublox_msgs::msg::CfgGNSSBlock::FLAGS_ENABLE) | enable_glonass_; RCLCPP_DEBUG(node_->get_logger(), "GLONASS Configuration is different"); } } diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index 91365de6..d362c8d0 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -148,7 +148,7 @@ namespace Class { static const uint8_t HNR = 0x28; //!< High Rate Navigation Results Messages: //!< High rate time, position, speed, heading static const uint8_t RTCM = 0xF5; //!< RTCM Configuration Messages -} +} // namespace Class namespace Message { namespace NAV { @@ -171,7 +171,7 @@ namespace Message { static const uint8_t TIMEUTC = ublox_msgs::msg::NavTIMEUTC::MESSAGE_ID; static const uint8_t VELECEF = ublox_msgs::msg::NavVELECEF::MESSAGE_ID; static const uint8_t VELNED = ublox_msgs::msg::NavVELNED::MESSAGE_ID; - } + } // namespace NAV namespace RXM { static const uint8_t ALM = ublox_msgs::msg::RxmALM::MESSAGE_ID; @@ -182,7 +182,7 @@ namespace Message { static const uint8_t SFRB = ublox_msgs::msg::RxmSFRB::MESSAGE_ID; static const uint8_t SFRBX = ublox_msgs::msg::RxmSFRBX::MESSAGE_ID; static const uint8_t SVSI = ublox_msgs::msg::RxmSVSI::MESSAGE_ID; - } + } // namespace RXM namespace INF { static const uint8_t ERROR = 0x00; @@ -190,18 +190,18 @@ namespace Message { static const uint8_t NOTICE = 0x02; static const uint8_t TEST = 0x03; static const uint8_t DEBUG = 0x04; - } + } // namespace INF namespace ACK { static const uint8_t NACK = 0x00; static const uint8_t ACK = 0x01; - } + } // namespace ACK namespace AID { static const uint8_t ALM = ublox_msgs::msg::AidALM::MESSAGE_ID; static const uint8_t EPH = ublox_msgs::msg::AidEPH::MESSAGE_ID; static const uint8_t HUI = ublox_msgs::msg::AidHUI::MESSAGE_ID; - } + } // namespace AID namespace CFG { static const uint8_t ANT = ublox_msgs::msg::CfgANT::MESSAGE_ID; @@ -221,39 +221,39 @@ namespace Message { static const uint8_t SBAS = ublox_msgs::msg::CfgSBAS::MESSAGE_ID; static const uint8_t TMODE3 = ublox_msgs::msg::CfgTMODE3::MESSAGE_ID; static const uint8_t USB = ublox_msgs::msg::CfgUSB::MESSAGE_ID; - } + } // namespace CFG namespace UPD { //! SOS and SOS_Ack have the same message ID, but different lengths static const uint8_t SOS = ublox_msgs::msg::UpdSOS::MESSAGE_ID; - } + } // namespace UPD namespace MON { static const uint8_t GNSS = ublox_msgs::msg::MonGNSS::MESSAGE_ID; static const uint8_t HW = ublox_msgs::msg::MonHW::MESSAGE_ID; static const uint8_t VER = ublox_msgs::msg::MonVER::MESSAGE_ID; - } + } // namespace MON namespace ESF { static const uint8_t INS = ublox_msgs::msg::EsfINS::MESSAGE_ID; static const uint8_t MEAS = ublox_msgs::msg::EsfMEAS::MESSAGE_ID; static const uint8_t RAW = ublox_msgs::msg::EsfRAW::MESSAGE_ID; static const uint8_t STATUS = ublox_msgs::msg::EsfSTATUS::MESSAGE_ID; - } + } // namespace ESF namespace MGA { static const uint8_t GAL = ublox_msgs::msg::MgaGAL::MESSAGE_ID; - } + } // namespace MGA namespace HNR { static const uint8_t PVT = ublox_msgs::msg::HnrPVT::MESSAGE_ID; - } + } // namespace HNR namespace TIM { static const uint8_t TM2 = ublox_msgs::msg::TimTM2::MESSAGE_ID; - } -} + } // namespace TIM +} // namespace Message -} //!< namespace ublox_msgs +} // namespace ublox_msgs #endif //!< UBLOX_MSGS_UBLOX_MSGS_HPP diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 3dce5c15..b04fc81e 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -113,7 +113,8 @@ struct UbloxSerializer::valu v = *reinterpret_cast(stream.advance(sizeof(v))); } - inline static uint32_t serializedLength(const T&) { + inline static uint32_t serializedLength(const T& v) { + (void)v; return sizeof(T); } }; @@ -164,23 +165,20 @@ struct StdArrayUbloxSerializer::value || std::is_same::value>::type> { - typedef std::array ArrayType; - typedef typename ArrayType::iterator IteratorType; - typedef typename ArrayType::const_iterator ConstIteratorType; - template - inline static void write(Stream& stream, const ArrayType& v) { + inline static void write(Stream& stream, const std::array& v) { const uint32_t data_len = N * sizeof(T); std::memcpy(stream.advance(data_len), &v.front(), data_len); } template - inline static void read(Stream& stream, ArrayType& v) { + inline static void read(Stream& stream, std::array& v) { const uint32_t data_len = N * sizeof(T); std::memcpy(&v.front(), stream.advance(data_len), data_len); } - inline static uint32_t serializedLength(const ArrayType& v) { + inline static uint32_t serializedLength(const std::array& v) { + (void)v; return N * sizeof(T); } }; @@ -315,7 +313,7 @@ class Message { * @param message_id the message ID of the u-blox message */ static void addKey(uint8_t class_id, uint8_t message_id) { - keys_.push_back(std::make_pair(class_id, message_id)); + keys_.emplace_back(std::make_pair(class_id, message_id)); } struct StaticKeyInitializer @@ -371,7 +369,7 @@ class Reader { const Options &options = Options()) : data_(data), count_(count), found_(false), options_(options) {} - typedef const uint8_t *iterator; + using iterator = const uint8_t *; /** * @brief Search the buffer for the beginning of the next u-blox message @@ -550,7 +548,7 @@ class Reader { */ class Writer { public: - typedef uint8_t *iterator; + using iterator = uint8_t *; /** * @brief Construct a Writer with the given buffer. @@ -581,7 +579,7 @@ class Writer { // Encode the message and add it to the buffer UbloxSerializer::write(data_ + options_.header_length, size_ - options_.header_length, message); - return write(0, length, class_id, message_id); + return write(nullptr, length, class_id, message_id); } /** From f1c63d2c4e49fb32b71a1f66dab4821477f5c1df Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 7 Jan 2020 16:42:36 -0500 Subject: [PATCH 105/152] Make ublox_gps composable. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 22 +++++++++++++++++++--- ublox_gps/include/ublox_gps/node.hpp | 2 +- ublox_gps/package.xml | 1 + ublox_gps/src/node.cpp | 7 ++++++- ublox_gps/src/node_main.cpp | 4 +--- 5 files changed, 28 insertions(+), 8 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index d5e6c3e8..47c9b6a4 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(ublox_msgs REQUIRED) @@ -27,7 +28,7 @@ find_package(ublox_serialization REQUIRED) include_directories(include) # build node -add_executable(ublox_gps_node +add_library(ublox_gps src/adr_udr_product.cpp src/gnss.cpp src/gps.cpp @@ -36,7 +37,6 @@ add_executable(ublox_gps_node src/hpg_rov_product.cpp src/mkgmtime.c src/node.cpp - src/node_main.cpp src/raw_data_pa.cpp src/raw_data_product.cpp src/tim_product.cpp @@ -45,19 +45,32 @@ add_executable(ublox_gps_node src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp) -ament_target_dependencies(ublox_gps_node +ament_target_dependencies(ublox_gps "asio" "diagnostic_msgs" "diagnostic_updater" "geometry_msgs" "rcl_interfaces" "rclcpp" + "rclcpp_components" "sensor_msgs" "std_msgs" "ublox_msgs" "ublox_serialization" ) +install(TARGETS ublox_gps + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +add_executable(ublox_gps_node src/node_main.cpp) +ament_target_dependencies(ublox_gps_node + "rclcpp" +) +target_link_libraries(ublox_gps_node ublox_gps) + # build logger node add_executable(ublox_logger_node src/logger_node_pa.cpp src/raw_data_pa.cpp) set_target_properties(ublox_logger_node PROPERTIES OUTPUT_NAME ublox_logger) @@ -71,6 +84,9 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +rclcpp_components_register_nodes(ublox_gps + "ublox_node::UbloxNode") + install(DIRECTORY include/ DESTINATION include ) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index a2745711..d40c8428 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -97,7 +97,7 @@ class UbloxNode final : public rclcpp::Node { /** * @brief Initialize and run the u-blox node. */ - UbloxNode(); + explicit UbloxNode(const rclcpp::NodeOptions & options); ~UbloxNode() override; diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 62dcc428..f4bd2ba4 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -20,6 +20,7 @@ geometry_msgs rcl_interfaces rclcpp + rclcpp_components sensor_msgs std_msgs ublox_msgs diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 5df714fa..ee133172 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -39,6 +39,9 @@ #include #include +#include +#include + #include #include #include @@ -169,7 +172,7 @@ std::vector stringSplit(const std::string &str, // // u-blox ROS Node // -UbloxNode::UbloxNode() : rclcpp::Node("ublox_gps_node") { +UbloxNode::UbloxNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ublox_gps_node", options) { int debug = this->declare_parameter("debug", 1); if (debug) { if (rcutils_logging_set_logger_level("ublox_gps_node", RCUTILS_LOG_SEVERITY_DEBUG) != RCUTILS_RET_OK) { @@ -834,3 +837,5 @@ UbloxNode::~UbloxNode() { } } // namespace ublox_node + +RCLCPP_COMPONENTS_REGISTER_NODE(ublox_node::UbloxNode) diff --git a/ublox_gps/src/node_main.cpp b/ublox_gps/src/node_main.cpp index 7363d40a..aeb38bfe 100644 --- a/ublox_gps/src/node_main.cpp +++ b/ublox_gps/src/node_main.cpp @@ -10,9 +10,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::spin(node); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); From 9006132278f4fa5877a42c9f4e8e68c6cf0ff830 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 7 Jan 2020 17:11:32 -0500 Subject: [PATCH 106/152] Fix heading output to comply with REP-103 This is a port of #72 to ROS 2. Signed-off-by: Chris Lalancette --- ublox_gps/src/hp_pos_rec_product.cpp | 11 ++++++--- ublox_msgs/msg/NavRELPOSNED.msg | 22 +++++++++--------- ublox_msgs/msg/NavRELPOSNED9.msg | 34 ++++++++++++++-------------- 3 files changed, 36 insertions(+), 31 deletions(-) diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index 4d3ae135..e25d37c8 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -49,17 +49,22 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 imu_.linear_acceleration_covariance[0] = -1; imu_.angular_velocity_covariance[0] = -1; - double heading = static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI; + // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). + // Alos convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id). + double heading = (static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI) - M_PI_2; tf2::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; imu_.orientation.y = orientation[1]; imu_.orientation.z = orientation[2]; imu_.orientation.w = orientation[3]; - // Only heading is reported with an accuracy in 0.1mm units imu_.orientation_covariance[0] = 1000.0; imu_.orientation_covariance[4] = 1000.0; - imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2); + imu_.orientation_covariance[8] = 1000.0; + // When heading is reported to be valid, use accuracy reported in 1e-5 deg units + if (m.flags & ublox_msgs::msg::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID) { + imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2); + } imu_pub_->publish(imu_); } diff --git a/ublox_msgs/msg/NavRELPOSNED.msg b/ublox_msgs/msg/NavRELPOSNED.msg index 14b574ba..6f443bda 100644 --- a/ublox_msgs/msg/NavRELPOSNED.msg +++ b/ublox_msgs/msg/NavRELPOSNED.msg @@ -18,31 +18,31 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved0 # Reserved -uint16 ref_station_id # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 i_tow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 rel_pos_n # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 rel_pos_e # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 rel_pos_d # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int8 rel_pos_hpn # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 rel_pos_hpe # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 rel_pos_hpd # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative @@ -51,11 +51,11 @@ int8 rel_pos_hpd # High-precision Down component of relative uint8 reserved1 # Reserved -uint32 acc_n # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 acc_e # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 acc_d # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] uint32 flags diff --git a/ublox_msgs/msg/NavRELPOSNED9.msg b/ublox_msgs/msg/NavRELPOSNED9.msg index 13f3e89d..1a7e6394 100644 --- a/ublox_msgs/msg/NavRELPOSNED9.msg +++ b/ublox_msgs/msg/NavRELPOSNED9.msg @@ -17,57 +17,57 @@ uint8 MESSAGE_ID = 60 uint8 version # Message version (0x00 for this version) uint8 reserved1 # Reserved -uint16 ref_station_id # Reference Station ID. Must be in the range +uint16 ref_station_id # Reference Station ID. Must be in the range # 0..4095 -uint32 i_tow # GPS time of week of the navigation epoch +uint32 i_tow # GPS time of week of the navigation epoch # [ms] -int32 rel_pos_n # North component of relative position vector +int32 rel_pos_n # North component of relative position vector # [cm] -int32 rel_pos_e # East component of relative position vector +int32 rel_pos_e # East component of relative position vector # [cm] -int32 rel_pos_d # Down component of relative position vector +int32 rel_pos_d # Down component of relative position vector # [cm] -int32 rel_pos_length # Length of the relative position vector +int32 rel_pos_length # Length of the relative position vector # [cm] -int32 rel_pos_heading # Heading of the relative position vector +int32 rel_pos_heading # Heading of the relative position vector # [1e-5 deg] uint8[4] reserved2 # Reserved -int8 rel_pos_hpn # High-precision North component of relative +int8 rel_pos_hpn # High-precision North component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full North component of the relative # position vector, in units of cm, is given by # relPosN + (relPosHPN * 1e-2) -int8 rel_pos_hpe # High-precision East component of relative +int8 rel_pos_hpe # High-precision East component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full East component of the relative # position vector, in units of cm, is given by # relPosE + (relPosHPE * 1e-2) -int8 rel_pos_hpd # High-precision Down component of relative +int8 rel_pos_hpd # High-precision Down component of relative # position vector. [0.1 mm] # Must be in the range -99 to +99. # The full Down component of the relative # position vector, in units of cm, is given by # relPosD + (relPosHPD * 1e-2) -int8 rel_pos_hp_length # High-precision component of the length of +int8 rel_pos_hp_length # High-precision component of the length of # the relative position vector. # Must be in the range -99 to +99. # The full length of the relative position # vector, in units of cm, is given by # relPosLength + (relPosHPLength * 1e-2) -uint32 acc_n # Accuracy of relative position North +uint32 acc_n # Accuracy of relative position North # component [0.1 mm] -uint32 acc_e # Accuracy of relative position East component +uint32 acc_e # Accuracy of relative position East component # [0.1 mm] -uint32 acc_d # Accuracy of relative position Down component +uint32 acc_d # Accuracy of relative position Down component # [0.1 mm] -uint32 acc_length # Accuracy of length of the relative position - # vector [0.1 mm] -uint32 acc_heading # Accuracy of heading of the relative position +uint32 acc_length # Accuracy of length of the relative position # vector [0.1 mm] +uint32 acc_heading # Accuracy of heading of the relative position + # vector [1e-5 deg] uint8[4] reserved3 # Reserved From 925f76d82aec38884e99bffff32160a5ff241759 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 7 Jan 2020 17:42:06 -0500 Subject: [PATCH 107/152] Finish porting configs to ROS 2. Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_base.yaml | 122 ++++++++++++++-------------- ublox_gps/config/c94_m8t_base.yaml | 41 +++++----- ublox_gps/config/c94_m8t_rover.yaml | 94 ++++++++++----------- ublox_gps/config/m8u_rover.yaml | 46 ----------- ublox_gps/config/neo_m8u_rover.yaml | 48 +++++++++++ 5 files changed, 179 insertions(+), 172 deletions(-) delete mode 100644 ublox_gps/config/m8u_rover.yaml create mode 100644 ublox_gps/config/neo_m8u_rover.yaml diff --git a/ublox_gps/config/c94_m8p_base.yaml b/ublox_gps/config/c94_m8p_base.yaml index dac60a47..5a6f6556 100644 --- a/ublox_gps/config/c94_m8p_base.yaml +++ b/ublox_gps/config/c94_m8p_base.yaml @@ -1,72 +1,74 @@ # Configuration Settings for C94-M8P device +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM + device: 4 # Save to EEPROM + + device: /dev/ttyACM0 + frame_id: gps_base + dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics + # assumed. + fix_mode: auto + dr_limit: 0 + enable_ppp: false # Not supported by C94-M8P -device: /dev/ttyACM0 -frame_id: gps_base -dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics - # assumed. -fix_mode: auto -dr_limit: 0 -enable_ppp: false # Not supported by C94-M8P + rate: 4 # Measurement rate in Hz + nav_rate: 4 # in number of measurement cycles -rate: 4 # Measurement rate in Hz -nav_rate: 4 # in number of measurement cycles + uart1: + baudrate: 19200 # C94-M8P specific + in: 0 # No UART in for base + out: 32 # RTCM 3 -uart1: - baudrate: 19200 # C94-M8P specific - in: 0 # No UART in for base - out: 32 # RTCM 3 + # TMODE3 Config + tmode3: 1 # Survey-In Mode + sv_in: + reset: false # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] -# TMODE3 Config -tmode3: 1 # Survey-In Mode -sv_in: - reset: false # True: disables and re-enables survey-in (resets) - # False: Disables survey-in only if TMODE3 is - # disabled - min_dur: 300 # Survey-In Minimum Duration [s] - acc_lim: 3.0 # Survey-In Accuracy Limit [m] + # RTCM out config + rtcm: + ids: [5, 87, 77, 230] # RTCM Messages to configure for Base station + # Enabled: GPS MSM7, GLONASS MSM7, + # GLONASS CP bias, Stationary RTK ref + # 0xF5 0x05 Stationary RTK reference station + # ARP + # 0xF5 0x4A GPS MSM4 + # 0xF5 0x4D GPS MSM7 + # 0xF5 0x54 GLONASS MSM4 + # 0xF5 0x57 GLONASS MSM7 + # 0xF5 0x7C BeiDou MSM4 + # 0xF5 0x7F BeiDou MSM7 + # 0xF5 0xE6 GLONASS code-phase biases + # 0xF5 0xFE Reference station PVT + #(u-blox proprietary RTCM Message) + rates: [1, 1, 1, 10] # in number of navigation solutions, must match + # nav_rate, except for 0xE6 -# RTCM out config -rtcm: - ids: [5, 87, 77, 230] # RTCM Messages to configure for Base station - # Enabled: GPS MSM7, GLONASS MSM7, - # GLONASS CP bias, Stationary RTK ref - # 0xF5 0x05 Stationary RTK reference station - # ARP - # 0xF5 0x4A GPS MSM4 - # 0xF5 0x4D GPS MSM7 - # 0xF5 0x54 GLONASS MSM4 - # 0xF5 0x57 GLONASS MSM7 - # 0xF5 0x7C BeiDou MSM4 - # 0xF5 0x7F BeiDou MSM7 - # 0xF5 0xE6 GLONASS code-phase biases - # 0xF5 0xFE Reference station PVT - #(u-blox proprietary RTCM Message) - rates: [1, 1, 1, 10] # in number of navigation solutions, must match - # nav_rate, except for 0xE6 + dat: + set: false -dat: - set: false + # GNSS Config + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: false # Supported by C94-M8P -# GNSS Config -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P + inf: + all: true # Whether to display all INF messages in console -inf: - all: true # Whether to display all INF messages in console + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + nav: + posecef: false -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - nav: - posecef: false diff --git a/ublox_gps/config/c94_m8t_base.yaml b/ublox_gps/config/c94_m8t_base.yaml index d7c9dfe4..42c9a93f 100644 --- a/ublox_gps/config/c94_m8t_base.yaml +++ b/ublox_gps/config/c94_m8t_base.yaml @@ -1,25 +1,26 @@ -# Configuration Settings for 4-M8P device +# Configuration Settings for C94-M8T device +ublox_gps_node: + ros__parameters: + debug: 2 # Range 0-4 (0 means no debug statements will print) -debug: 2 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: m8t_base + dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics + # assumed. + fix_mode: auto -device: /dev/ttyACM0 -frame_id: m8t_base -dynamic_model: stationary # Velocity restricted to 0 m/s. Zero dynamics - # assumed. -fix_mode: auto + rate: 1 # Measurement rate in Hz + nav_rate: 1 # in number of measurement cycles -rate: 1 # Measurement rate in Hz -nav_rate: 1 # in number of measurement cycles + uart1: + baudrate: 9600 # C94-M8P specific + in: 0 # No UART in for base + out: 0 # RTCM 3 -uart1: - baudrate: 9600 # C94-M8P specific - in: 0 # No UART in for base - out: 0 # RTCM 3 + inf: + all: true # Whether to display all INF messages in console -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - tim: - tm2: true + # Enable u-blox message publishers + publish: + tim: + tm2: true diff --git a/ublox_gps/config/c94_m8t_rover.yaml b/ublox_gps/config/c94_m8t_rover.yaml index f154b45d..741fa28e 100644 --- a/ublox_gps/config/c94_m8t_rover.yaml +++ b/ublox_gps/config/c94_m8t_rover.yaml @@ -1,46 +1,48 @@ -# Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM1 -frame_id: gps -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: auto -enable_ppp: false # Not supported by C94-M8P -dr_limit: 0 - -uart1: - baudrate: 19200 # C94-M8P specific - in: 32 # RTCM 3 - out: 0 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - aid: - hui: false - - nav: - posecef: false \ No newline at end of file +# Configuration Settings for C94-M8T device +ublox_gps_node: + ros__parameters: + + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM1 + frame_id: gps + rate: 4 # in Hz + nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: auto + enable_ppp: false # Not supported by C94-M8T + dr_limit: 0 + + uart1: + baudrate: 19200 # C94-M8P specific + in: 32 # RTCM 3 + out: 0 # No UART out for rover + + gnss: + glonass: true # Supported by C94-M8P + beidou: false # Supported by C94-M8P + qzss: false # Supported by C94-M8P + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + aid: + hui: false + + nav: + posecef: false diff --git a/ublox_gps/config/m8u_rover.yaml b/ublox_gps/config/m8u_rover.yaml deleted file mode 100644 index 5f57df95..00000000 --- a/ublox_gps/config/m8u_rover.yaml +++ /dev/null @@ -1,46 +0,0 @@ -# Configuration Settings for C94-M8P device - -debug: 1 # Range 0-4 (0 means no debug statements will print) - -save: - mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration - device: 4 # Save to EEPROM - -device: /dev/ttyACM0 -frame_id: m8u -rate: 4 # in Hz -nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may - # be either 5 Hz (Dual constellation) or - # 8 Hz (GPS only) -dynamic_model: 0 # Airborne < 2G, 2D fix not supported (3D only), - # Max Alt: 50km - # Max Horizontal Velocity: 250 m/s, - # Max Vertical Velocity: 100 m/s -fix_mode: 3 -enable_ppp: true # Not supported by C94-M8P -dr_limit: 1 - -uart1: - baudrate: 115200 # C94-M8P specific - in: 16 # RTCM 3 - out: 16 # No UART out for rover - -gnss: - glonass: true # Supported by C94-M8P - beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P - -dgnss_mode: 3 # Fixed mode - -inf: - all: true # Whether to display all INF messages in console - -# Enable u-blox message publishers -publish: - all: true - esf: true - aid: - hui: false - nav: - posecef: false diff --git a/ublox_gps/config/neo_m8u_rover.yaml b/ublox_gps/config/neo_m8u_rover.yaml new file mode 100644 index 00000000..dc364feb --- /dev/null +++ b/ublox_gps/config/neo_m8u_rover.yaml @@ -0,0 +1,48 @@ +# Configuration Settings for NEO-M8U device + +ublox_gps_node: + ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) + + save: + mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver + # Manager, Antenna, and Logging Configuration + device: 4 # Save to EEPROM + + device: /dev/ttyACM0 + frame_id: m8u + rate: 4 # in Hz + nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + # be either 5 Hz (Dual constellation) or + # 8 Hz (GPS only) + dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + # Max Alt: 50km + # Max Horizontal Velocity: 250 m/s, + # Max Vertical Velocity: 100 m/s + fix_mode: 3 + enable_ppp: true + dr_limit: 1 + + uart1: + baudrate: 115200 # NEO-M8U specific + in: 16 # RTCM 3 + out: 16 # No UART out for rover + + gnss: + glonass: true # Supported by NEO-M8U + beidou: false # Supported by NEO-M8U + qzss: false # Supported by NEO-M8U + + dgnss_mode: 3 # Fixed mode + + inf: + all: true # Whether to display all INF messages in console + + # Enable u-blox message publishers + publish: + all: true + esf: true + aid: + hui: false + nav: + posecef: false From 87290a9eb8562aea33c635b6583a3cefc5dded1e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 7 Jan 2020 17:56:42 -0500 Subject: [PATCH 108/152] Create ROS 2 launch files. Signed-off-by: Chris Lalancette --- ublox_gps/launch/ublox_device.launch | 19 ------ .../launch/ublox_gps_node-composed-launch.py | 68 +++++++++++++++++++ ublox_gps/launch/ublox_gps_node-launch.py | 60 ++++++++++++++++ 3 files changed, 128 insertions(+), 19 deletions(-) delete mode 100644 ublox_gps/launch/ublox_device.launch create mode 100644 ublox_gps/launch/ublox_gps_node-composed-launch.py create mode 100644 ublox_gps/launch/ublox_gps_node-launch.py diff --git a/ublox_gps/launch/ublox_device.launch b/ublox_gps/launch/ublox_device.launch deleted file mode 100644 index 9bd8ce10..00000000 --- a/ublox_gps/launch/ublox_device.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - diff --git a/ublox_gps/launch/ublox_gps_node-composed-launch.py b/ublox_gps/launch/ublox_gps_node-composed-launch.py new file mode 100644 index 00000000..f0be0099 --- /dev/null +++ b/ublox_gps/launch/ublox_gps_node-composed-launch.py @@ -0,0 +1,68 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 {copyright_holder} 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 THE +# COPYRIGHT OWNER OR CONTRIBUTORS 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. + +"""Launch the ublox gps node in a composable container with c94-m8p configuration.""" + +import os + +import ament_index_python.packages + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import yaml + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ublox_gps'), + 'config') + param_config = os.path.join(config_directory, 'c94_m8p_rover.yaml') + with open(param_config, 'r') as f: + params = yaml.safe_load(f)['ublox_gps_node']['ros__parameters'] + container = ComposableNodeContainer( + node_name='ublox_gps_container', + node_namespace='', + package='rclcpp_components', + node_executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='ublox_gps', + node_plugin='ublox_node::UbloxNode', + node_name='ublox_gps_node', + parameters=[params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/ublox_gps/launch/ublox_gps_node-launch.py b/ublox_gps/launch/ublox_gps_node-launch.py new file mode 100644 index 00000000..7f0da280 --- /dev/null +++ b/ublox_gps/launch/ublox_gps_node-launch.py @@ -0,0 +1,60 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 {copyright_holder} 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 THE +# COPYRIGHT OWNER OR CONTRIBUTORS 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. + +"""Launch the ublox gps node with c94-m8p configuration.""" + +import os + +import ament_index_python.packages +import launch +import launch_ros.actions + + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ublox_gps'), + 'config') + params = os.path.join(config_directory, 'c94_m8p_rover.yaml') + ublox_gps_node = launch_ros.actions.Node(package='ublox_gps', + node_executable='ublox_gps_node', + output='both', + parameters=[params]) + + return launch.LaunchDescription([ublox_gps_node, + + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=velodyne_driver_node, + on_exit=[launch.actions.EmitEvent( + event=launch.events.Shutdown())], + )), + ]) From 98fd4df101dc223fe57596ff4523365c35a3379b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Jan 2020 19:58:35 +0000 Subject: [PATCH 109/152] Update rover configuration. Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_rover.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ublox_gps/config/c94_m8p_rover.yaml b/ublox_gps/config/c94_m8p_rover.yaml index 20fa5b2f..cd6f16cd 100644 --- a/ublox_gps/config/c94_m8p_rover.yaml +++ b/ublox_gps/config/c94_m8p_rover.yaml @@ -10,11 +10,11 @@ ublox_gps_node: device: /dev/ttyACM0 frame_id: gps - rate: 4.0 # in Hz - nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may + rate: 1.0 # in Hz + nav_rate: 1 # [# of measurement cycles], recommended 1 Hz, may # be either 5 Hz (Dual constellation) or # 8 Hz (GPS only) - dynamic_model: airborne2 # Airborne < 2G, 2D fix not supported (3D only), + dynamic_model: portable # Airborne < 2G, 2D fix not supported (3D only), # Max Alt: 50km # Max Horizontal Velocity: 250 m/s, # Max Vertical Velocity: 100 m/s @@ -23,7 +23,7 @@ ublox_gps_node: dr_limit: 0 # TMODE3 Config - tmode3: 1 # Survey-In Mode + tmode3: 0 # Survey-In Mode sv_in: reset: false # True: disables and re-enables survey-in (resets) # False: Disables survey-in only if TMODE3 is @@ -39,7 +39,7 @@ ublox_gps_node: gnss: glonass: true # Supported by C94-M8P beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P + qzss: true # Supported by C94-M8P dgnss_mode: 3 # Fixed mode From 3dc6f5a68005f352b1257c0ff7a51e7447e838c6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Jan 2020 21:44:12 +0000 Subject: [PATCH 110/152] Configure the base more accurately. Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_base.yaml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ublox_gps/config/c94_m8p_base.yaml b/ublox_gps/config/c94_m8p_base.yaml index 5a6f6556..77614ea9 100644 --- a/ublox_gps/config/c94_m8p_base.yaml +++ b/ublox_gps/config/c94_m8p_base.yaml @@ -5,7 +5,7 @@ ublox_gps_node: save: mask: 3103 # Save I/O, Message, INF Message, Nav, Receiver - # Manager, Antenna, and Logging Configuration + # Manager, Antenna, and Logging Configuration device: 4 # Save to EEPROM device: /dev/ttyACM0 @@ -16,8 +16,8 @@ ublox_gps_node: dr_limit: 0 enable_ppp: false # Not supported by C94-M8P - rate: 4 # Measurement rate in Hz - nav_rate: 4 # in number of measurement cycles + rate: 1.0 # Measurement rate in Hz + nav_rate: 1 # in number of measurement cycles uart1: baudrate: 19200 # C94-M8P specific @@ -59,7 +59,7 @@ ublox_gps_node: gnss: glonass: true # Supported by C94-M8P beidou: false # Supported by C94-M8P - qzss: false # Supported by C94-M8P + qzss: true # Supported by C94-M8P inf: all: true # Whether to display all INF messages in console @@ -71,4 +71,3 @@ ublox_gps_node: hui: false nav: posecef: false - From 048f4c2a53364dd45657795c36fd2f9ba01f7630 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Jan 2020 21:44:34 +0000 Subject: [PATCH 111/152] Increase the timeout for NavPVT messages during Survey-In config. Signed-off-by: Chris Lalancette --- ublox_gps/src/hpg_ref_product.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index d632597f..90f5f1b7 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -128,7 +129,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr gps) { return true; } ublox_msgs::msg::NavPVT nav_pvt; - if (!gps->poll(nav_pvt)) { + if (!gps->poll(nav_pvt, std::vector(), std::chrono::milliseconds(15000))) { throw std::runtime_error(std::string("Failed to poll NavPVT while") + " configuring survey-in"); } @@ -195,8 +196,8 @@ bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { // Set the Measurement & nav rate to user config // (survey-in sets nav_rate to 1 Hz regardless of user setting) if (!gps->configRate(meas_rate_, nav_rate_)) { - RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms %s %d", meas_rate_, - "navigation rate to ", nav_rate_); + RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms navigation rate to %d cycles", + meas_rate_, nav_rate_); } // Enable the RTCM out messages if (!gps->configRtcm(rtcms_)) { From a6975905f4146196f9dc59639245ac5460e3b898 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 16 Jan 2020 21:44:55 +0000 Subject: [PATCH 112/152] Fix getting RTCM ids and rates. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index ee133172..26ee4f58 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -278,12 +278,13 @@ void UbloxNode::getRosParams() { nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles // RTCM params - std::vector rtcm_ids; - std::vector rtcm_rates; this->declare_parameter("rtcm.ids"); this->declare_parameter("rtcm.rates"); - getRosUint(this, "rtcm.ids", rtcm_ids); // RTCM output message IDs - getRosUint(this, "rtcm.rates", rtcm_rates); // RTCM output message rates + std::vector rtcm_ids; + std::vector rtcm_rates; + this->get_parameter("rtcm.ids", rtcm_ids); + this->get_parameter("rtcm.rates", rtcm_rates); + if (rtcm_ids.size() != rtcm_rates.size()) { throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); @@ -291,6 +292,12 @@ void UbloxNode::getRosParams() { rtcms_.resize(rtcm_ids.size()); for (size_t i = 0; i < rtcm_ids.size(); ++i) { + if (rtcm_ids[i] < 0 || rtcm_ids[i] > 255) { + throw std::runtime_error("RTCM IDs must be between 0 and 255"); + } + if (rtcm_rates[i] < 0 || rtcm_rates[i] > 255) { + throw std::runtime_error("RTCM rates must be between 0 and 255"); + } rtcms_[i].id = rtcm_ids[i]; rtcms_[i].rate = rtcm_rates[i]; } From 22e727bdb20b709a670d349c6fd1510b06c0545f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 18 Feb 2020 21:59:23 +0000 Subject: [PATCH 113/152] Make sure to have package.xml depend on ament_cmake_ros Signed-off-by: Chris Lalancette --- ublox_gps/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index f4bd2ba4..9a7b00aa 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -12,7 +12,7 @@ BSD http://ros.org/wiki/ublox - ament_cmake + ament_cmake_ros asio diagnostic_msgs From 32c1f7fd6ba7928bf10c9bb8d53459de89a5e602 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 18 Feb 2020 22:08:14 +0000 Subject: [PATCH 114/152] Make sure to depend on tf2 properly. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 2 ++ ublox_gps/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 47c9b6a4..98b00186 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) find_package(ublox_msgs REQUIRED) find_package(ublox_serialization REQUIRED) @@ -55,6 +56,7 @@ ament_target_dependencies(ublox_gps "rclcpp_components" "sensor_msgs" "std_msgs" + "tf2" "ublox_msgs" "ublox_serialization" ) diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 9a7b00aa..6162a5cd 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -23,6 +23,7 @@ rclcpp_components sensor_msgs std_msgs + tf2 ublox_msgs ublox_serialization From 8cb5d20768baa82b0e5d135c4964ddc74661f8c6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Feb 2020 19:31:48 +0000 Subject: [PATCH 115/152] Make sure to reset for survey-in mode. Signed-off-by: Chris Lalancette --- ublox_gps/config/c94_m8p_base.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/config/c94_m8p_base.yaml b/ublox_gps/config/c94_m8p_base.yaml index 77614ea9..a845728f 100644 --- a/ublox_gps/config/c94_m8p_base.yaml +++ b/ublox_gps/config/c94_m8p_base.yaml @@ -27,7 +27,7 @@ ublox_gps_node: # TMODE3 Config tmode3: 1 # Survey-In Mode sv_in: - reset: false # True: disables and re-enables survey-in (resets) + reset: true # True: disables and re-enables survey-in (resets) # False: Disables survey-in only if TMODE3 is # disabled min_dur: 300 # Survey-In Minimum Duration [s] From a581cb6e8453f9f3c1c6497dd1f301b305a4c338 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 23 Mar 2020 12:01:52 -0400 Subject: [PATCH 116/152] Create publishers based on ROS parameters (#1) * publishers created in conditionals in constructors * move publisher creations to after parameters have been declared --- ublox_gps/include/ublox_gps/node.hpp | 2 +- .../include/ublox_gps/ublox_firmware7.hpp | 8 +++- .../include/ublox_gps/ublox_firmware7plus.hpp | 4 +- .../include/ublox_gps/ublox_firmware8.hpp | 12 +++-- ublox_gps/src/adr_udr_product.cpp | 34 +++++++++---- ublox_gps/src/hp_pos_rec_product.cpp | 12 +++-- ublox_gps/src/hpg_ref_product.cpp | 6 ++- ublox_gps/src/hpg_rov_product.cpp | 6 ++- ublox_gps/src/node.cpp | 48 ++++++++++++++----- ublox_gps/src/raw_data_product.cpp | 16 +++++-- ublox_gps/src/tim_product.cpp | 17 ++++--- ublox_gps/src/ublox_firmware6.cpp | 33 ++++++++----- 12 files changed, 139 insertions(+), 59 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index d40c8428..4aeeb84f 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -243,7 +243,7 @@ class UbloxNode final : public rclcpp::Node { uint8_t tim_rate_{0}; //! raw data stream logging - RawDataStreamPa raw_data_stream_pa_; + std::shared_ptr raw_data_stream_pa_; rclcpp::Publisher::SharedPtr nav_status_pub_; rclcpp::Publisher::SharedPtr nav_posecef_pub_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp index 82b3e1bf..6f6f95d4 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7.hpp @@ -25,8 +25,12 @@ class UbloxFirmware7 final : public UbloxFirmware7Plus public: explicit UbloxFirmware7(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); - mon_hw_pub_ = node->create_publisher("monhw", 1); + if (getRosBoolean(node_, "publish.nav.svinfo")) { + nav_svinfo_pub_ = node->create_publisher("navsvinfo", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = node->create_publisher("monhw", 1); + } } /** diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp index f18fae1b..84ffe7a7 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -34,7 +34,9 @@ class UbloxFirmware7Plus : public UbloxFirmware { explicit UbloxFirmware7Plus(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher - nav_pvt_pub_ = node_->create_publisher("navpvt", 1); + if (getRosBoolean(node_, "publish.nav.pvt")) { + nav_pvt_pub_ = node_->create_publisher("navpvt", 1); + } fix_pub_ = node_->create_publisher("fix", 1); diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index 0f21db5a..85223544 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -25,9 +25,15 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { public: explicit UbloxFirmware8(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware7Plus(frame_id, updater, freq_diag, gnss, node) { - nav_sat_pub_ = node->create_publisher("navstate", 1); - mon_hw_pub_ = node->create_publisher("monhw", 1); - rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); + if (getRosBoolean(node_, "publish.nav.sat")) { + nav_sat_pub_ = node->create_publisher("navstate", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = node->create_publisher("monhw", 1); + } + if (getRosBoolean(node_, "publish.rxm.rtcm")) { + rxm_rtcm_pub_ = node->create_publisher("rxmrtcm", 1); + } } /** diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 1cbe8d6c..3a19c172 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -28,16 +28,29 @@ namespace ublox_node { AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { - imu_pub_ = - node_->create_publisher("imu_meas", 1); - time_ref_pub_ = - node_->create_publisher("interrupt_time", 1); - nav_att_pub_ = node_->create_publisher("navatt", 1); - esf_ins_pub_ = node_->create_publisher("esfins", 1); - esf_meas_pub_ = node_->create_publisher("esfmeas", 1); - esf_raw_pub_ = node_->create_publisher("esfraw", 1); - esf_status_pub_ = node_->create_publisher("esfstatus", 1); - hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); + if (getRosBoolean(node_, "publish.esf.meas")) { + imu_pub_ = + node_->create_publisher("imu_meas", 1); + time_ref_pub_ = + node_->create_publisher("interrupt_time", 1); + + esf_meas_pub_ = node_->create_publisher("esfmeas", 1); + } + if (getRosBoolean(node_, "publish.nav.att")) { + nav_att_pub_ = node_->create_publisher("navatt", 1); + } + if (getRosBoolean(node_, "publish.esf.ins")) { + esf_ins_pub_ = node_->create_publisher("esfins", 1); + } + if (getRosBoolean(node_, "publish.esf.raw")) { + esf_raw_pub_ = node_->create_publisher("esfraw", 1); + } + if (getRosBoolean(node_, "publish.esf.status")) { + esf_status_pub_ = node_->create_publisher("esfstatus", 1); + } + if (getRosBoolean(node_, "publish.hnr.pvt")) { + hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); + } } void AdrUdrProduct::getRosParams() { @@ -74,6 +87,7 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { if (getRosBoolean(node_, "publish.esf.meas")) { gps->subscribe([this](const ublox_msgs::msg::EsfMEAS &m) { esf_meas_pub_->publish(m); }, 1); + // also publish sensor_msgs::Imu gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index e25d37c8..3e0a44ca 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -23,11 +23,15 @@ namespace ublox_node { HpPosRecProduct::HpPosRecProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) : HpgRefProduct(nav_rate, meas_rate, updater, rtcms, node), frame_id_(frame_id) { - nav_relposned_pub_ = - node_->create_publisher("navrelposned", 1); + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_relposned_pub_ = + node_->create_publisher("navrelposned", 1); + } - imu_pub_ = - node_->create_publisher("navheading", 1); + if (getRosBoolean(node_, "publish.nav.heading")) { + imu_pub_ = + node_->create_publisher("navheading", 1); + } } void HpPosRecProduct::subscribe(std::shared_ptr gps) { diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index 90f5f1b7..b12caaff 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -25,8 +25,10 @@ namespace ublox_node { HpgRefProduct::HpgRefProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) : tmode3_(0), lla_flag_(false), fixed_pos_acc_(0.0), svin_reset_(false), sv_in_min_dur_(0), sv_in_acc_lim_(0.0), nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), rtcms_(rtcms), node_(node) { - navsvin_pub_ = - node_->create_publisher("navsvin", 1); + if (getRosBoolean(node_, "publish.nav.svin")) { + navsvin_pub_ = + node_->create_publisher("navsvin", 1); + } } /** diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp index 09f19d39..7b524d57 100644 --- a/ublox_gps/src/hpg_rov_product.cpp +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -23,8 +23,10 @@ namespace ublox_node { HpgRovProduct::HpgRovProduct(uint16_t nav_rate, std::shared_ptr updater, rclcpp::Node* node) : dgnss_mode_(ublox_msgs::msg::CfgDGNSS::DGNSS_MODE_RTK_FIXED), nav_rate_(nav_rate), updater_(updater), node_(node) { - nav_rel_pos_ned_pub_ = - node_->create_publisher("navrelposned", 1); + if (getRosBoolean(node_, "publish.nav.relposned")) { + nav_rel_pos_ned_pub_ = + node_->create_publisher("navrelposned", 1); + } } void HpgRovProduct::getRosParams() { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 26ee4f58..907de3c9 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -184,13 +184,6 @@ UbloxNode::UbloxNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ublox_ gnss_ = std::make_shared(); - nav_status_pub_ = this->create_publisher("navstatus", 1); - nav_posecef_pub_ = this->create_publisher("navposecef", 1); - nav_clock_pub_ = this->create_publisher("navclock", 1); - aid_alm_pub_ = this->create_publisher("aidalm", 1); - aid_eph_pub_ = this->create_publisher("aideph", 1); - aid_hui_pub_ = this->create_publisher("aidhui", 1); - updater_ = std::make_shared(this); updater_->setHardwareID("ublox"); @@ -378,7 +371,12 @@ void UbloxNode::getRosParams() { this->declare_parameter("dgnss_mode"); // raw data stream logging - raw_data_stream_pa_.getRosParams(); + this->declare_parameter("raw_data_stream.enable", false); + if (getRosBoolean(this, "raw_data_stream.enable")) { + raw_data_stream_pa_ = std::make_shared( + getRosBoolean(this, "raw_data_stream.enable")); + raw_data_stream_pa_->getRosParams(); + } // NMEA parameters this->declare_parameter("nmea.set", false); @@ -459,6 +457,29 @@ void UbloxNode::getRosParams() { this->declare_parameter("arp.lla_flag", false); this->declare_parameter("diagnostic_period", kDiagnosticPeriod); + + // Create publishers based on parameters + if (getRosBoolean(this, "publish.nav.status")) { + nav_status_pub_ = this->create_publisher("navstatus", 1); + } + if (getRosBoolean(this, "publish.nav.posecef")) { + nav_posecef_pub_ = this->create_publisher("navposecef", 1); + } + if (getRosBoolean(this, "publish.nav.clock")) { + nav_clock_pub_ = this->create_publisher("navclock", 1); + } + if (getRosBoolean(this, "publish.nav.clock")) { + nav_clock_pub_ = this->create_publisher("navclock", 1); + } + if (getRosBoolean(this, "publish.aid.alm")) { + aid_alm_pub_ = this->create_publisher("aidalm", 1); + } + if (getRosBoolean(this, "publish.aid.eph")) { + aid_eph_pub_ = this->create_publisher("aideph", 1); + } + if (getRosBoolean(this, "publish.aid.hui")) { + aid_hui_pub_ = this->create_publisher("aidhui", 1); + } } void UbloxNode::pollMessages() { @@ -789,10 +810,13 @@ void UbloxNode::initializeIo() { } // raw data stream logging - if (raw_data_stream_pa_.isEnabled()) { - gps_->setRawDataCallback( - std::bind(&RawDataStreamPa::ubloxCallback, &raw_data_stream_pa_, std::placeholders::_1, std::placeholders::_2)); - raw_data_stream_pa_.initialize(); + if (getRosBoolean(this, "raw_data_stream.enable")) { + if (raw_data_stream_pa_->isEnabled()) { + gps_->setRawDataCallback( + std::bind(&RawDataStreamPa::ubloxCallback, raw_data_stream_pa_.get(), + std::placeholders::_1, std::placeholders::_2)); + raw_data_stream_pa_->initialize(); + } } } diff --git a/ublox_gps/src/raw_data_product.cpp b/ublox_gps/src/raw_data_product.cpp index a2887eb2..23c953aa 100644 --- a/ublox_gps/src/raw_data_product.cpp +++ b/ublox_gps/src/raw_data_product.cpp @@ -20,10 +20,18 @@ namespace ublox_node { // RawDataProduct::RawDataProduct(uint16_t nav_rate, uint16_t meas_rate, std::shared_ptr updater, rclcpp::Node* node) : nav_rate_(nav_rate), meas_rate_(meas_rate), updater_(updater), node_(node) { - rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); - rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); - rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); - rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); + if (getRosBoolean(node_, "publish.rxm.raw")) { + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + } + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + } + if (getRosBoolean(node_, "publish.rxm.eph")) { + rxm_eph_pub_ = node_->create_publisher("rxmeph", 1); + } + if (getRosBoolean(node_, "publish.rxm.almRaw")) { + rxm_alm_pub_ = node_->create_publisher("rxmalm", 1); + } } void RawDataProduct::subscribe(std::shared_ptr gps) { diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp index 4f705cc4..35f1a9cd 100644 --- a/ublox_gps/src/tim_product.cpp +++ b/ublox_gps/src/tim_product.cpp @@ -26,8 +26,13 @@ TimProduct::TimProduct(const std::string & frame_id, std::shared_ptrcreate_publisher("timtm2", 1); interrupt_time_pub_ = node_->create_publisher("interrupt_time", 1); - rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); - rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + + if (getRosBoolean(node_, "publish.rxm.sfrb")) { + rxm_sfrb_pub_ = node_->create_publisher("rxmsfrb", 1); + } + if (getRosBoolean(node_, "publish.rxm.raw")) { + rxm_raw_pub_ = node_->create_publisher("rxmraw", 1); + } } void TimProduct::getRosParams() { @@ -59,11 +64,11 @@ void TimProduct::subscribe(std::shared_ptr gps) { 1); } - // Subscribe to RawX messages - if (getRosBoolean(node_, "publish.rxm.raw")) { - gps->subscribe([this](const ublox_msgs::msg::RxmRAWX &m) { rxm_raw_pub_->publish(m); }, + // Subscribe to RawX messages + if (getRosBoolean(node_, "publish.rxm.raw")) { + gps->subscribe([this](const ublox_msgs::msg::RxmRAWX &m) { rxm_raw_pub_->publish(m); }, 1); - } + } } void TimProduct::callbackTimTM2(const ublox_msgs::msg::TimTM2 &m) { diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index 82f556c3..0bb7f3b5 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -30,26 +30,35 @@ namespace ublox_node { UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node) : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { - nav_pos_llh_pub_ = - node_->create_publisher("navposllh", 1); + if (getRosBoolean(node_, "publish.nav.posllh")) { + nav_pos_llh_pub_ = + node_->create_publisher("navposllh", 1); + } + fix_pub_ = node_->create_publisher("fix", 1); - nav_vel_ned_pub_ = - node_->create_publisher("navvelned", 1); + if (getRosBoolean(node_, "publish.nav.velned")) { + nav_vel_ned_pub_ = + node_->create_publisher("navvelned", 1); + } vel_pub_ = node_->create_publisher("fix_velocity", 1); - nav_sol_pub_ = - node_->create_publisher("navsol", 1); - - nav_svinfo_pub_ = - node_->create_publisher("navinfo", 1); - - mon_hw_pub_ = - node_->create_publisher("monhw", 1); + if (getRosBoolean(node_, "publish.nav.sol")) { + nav_sol_pub_ = + node_->create_publisher("navsol", 1); + } + if (getRosBoolean(node_, "publish.nav.svinfo")) { + nav_svinfo_pub_ = + node_->create_publisher("navinfo", 1); + } + if (getRosBoolean(node_, "publish.mon.hw")) { + mon_hw_pub_ = + node_->create_publisher("monhw", 1); + } } void UbloxFirmware6::getRosParams() { From eb6f033693fc75877c4b382d724b2937be0a37f6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 9 Sep 2020 10:15:56 -0400 Subject: [PATCH 117/152] Fixes to compile on Foxy. Signed-off-by: Chris Lalancette --- ublox_msgs/CMakeLists.txt | 16 +++++++++------- ublox_msgs/package.xml | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 46a08336..35fdf76f 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -10,7 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -104,16 +104,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) include_directories(include) -add_library(${PROJECT_NAME}_lib SHARED src/ublox_msgs.cpp) +add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) +rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") ament_target_dependencies(${PROJECT_NAME}_lib "ublox_serialization" ) -rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}_lib) -ament_export_dependencies(rosidl_default_runtime std_msgs sensor_msgs ublox_serialization) -install(TARGETS ${PROJECT_NAME}_lib +install(TARGETS ${PROJECT_NAME}_lib EXPORT ${PROJECT_NAME}_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -123,4 +120,9 @@ install(DIRECTORY include/ DESTINATION include ) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_lib) +ament_export_targets(${PROJECT_NAME}_lib) +ament_export_dependencies(rosidl_default_runtime sensor_msgs std_msgs ublox_serialization) + ament_package() diff --git a/ublox_msgs/package.xml b/ublox_msgs/package.xml index 89bdce08..62c61ac0 100644 --- a/ublox_msgs/package.xml +++ b/ublox_msgs/package.xml @@ -13,7 +13,7 @@ BSD http://ros.org/wiki/ublox - ament_cmake + ament_cmake_ros rosidl_default_generators From 5333d5ed1f1943115ec938cc627ad6b8474ec23c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Oct 2020 18:42:14 +0000 Subject: [PATCH 118/152] Changelog. Signed-off-by: Chris Lalancette --- ublox/CHANGELOG.rst | 7 ++ ublox_gps/CHANGELOG.rst | 112 ++++++++++++++++++++++++++++++ ublox_msgs/CHANGELOG.rst | 17 +++++ ublox_serialization/CHANGELOG.rst | 15 ++++ 4 files changed, 151 insertions(+) diff --git a/ublox/CHANGELOG.rst b/ublox/CHANGELOG.rst index e5c7e907..8b11883f 100644 --- a/ublox/CHANGELOG.rst +++ b/ublox/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ublox ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Initial ROS 2 port to Dashing +* Port the ublox package to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ diff --git a/ublox_gps/CHANGELOG.rst b/ublox_gps/CHANGELOG.rst index 7c0390d3..565ec4a0 100644 --- a/ublox_gps/CHANGELOG.rst +++ b/ublox_gps/CHANGELOG.rst @@ -2,6 +2,118 @@ Changelog for package ublox_gps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Initial ROS 2 port to Dashing +* Create publishers based on ROS parameters (`#1 `_) +* Make sure to reset for survey-in mode. +* Make sure to depend on tf2 properly. +* Make sure to have package.xml depend on ament_cmake_ros +* Fix getting RTCM ids and rates. +* Increase the timeout for NavPVT messages during Survey-In config. +* Configure the base more accurately. +* Update rover configuration. +* Create ROS 2 launch files. +* Finish porting configs to ROS 2. +* Fix heading output to comply with REP-103 +* Make ublox_gps composable. +* Fixes pointed out by clang-tidy. +* Rename rawDataStreamPa member variable to raw_data_stream_pa +* Fix a bug for reads of zero size. +* Re-enable logging in the AsyncWorker. +* Switch a couple more catch blocks to const. +* Remove -ggdb3 flag from CMakeLists.txt. +* Fix a few bugs pointed out by valgrind. +* Pass the logger down to the Gps level. +* Minor cleanup in the gps code. +* More fixes for declaring parameters. +* Declare more parameters. +* Declare more parameters correctly. +* More fixes so that we get the correct rates. +* More fixes around the codebase. +* Mark constants as such. +* Remove declareRosBoolean. +* Update configuration files for ROS 2. +* Port ublox_gps to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Split the main out into its own file. +* Move spinning out of the constructor. +* Cleanup includes in node.{hpp,cpp}. +* Move HpPosRecProduct class to its own files. +* Move HpgRefProduct class to its own files. +* Move UbloxFirmware9 class to its own files. +* Move UbloxFirmware8 class to its own files. +* Move UbloxFirmware7 to its own files. +* Move UbloxFirmware7Plus class to its own header file. +* Move UbloxFirmware6 class to its own files. +* Move UbloxFirmware class to its own files. +* Move RawDataProduct class into its own files. +* Move HpgRovProduct class to its own files. +* Move AdrUdrProduct class into its own files. +* Move TimProduct class into its own file. +* Make the node handle a member variable of UbloxNode. +* Remove the last uses of the global variable. +* Pass the nodehandle into more methods. +* Start passing the node into functions. +* Declare a few more booleans. +* Declare a lot more parameters. +* Finish removing the "enable" map. +* Move some more boolean parameters to declarations. +* Declare more parameters. +* Convert a few more parameters over to being declared. +* Declare the config_on_startup flag. +* Make dat/set a declared parameter. +* Make sure to declare the sbas parameter. +* Add in ROS2-like declare and get parameters. +* Replace templated publish call with lambdas. +* Make fix_status_service a member variable of UbloxFirmware. +* Move fixFromString and modelFromString into node.cpp +* Make gps a member variable of UbloxNode. +* Add namespaces to component_interface and fts_product.hpp +* Minor code improvements. +* Move FixDiagnostic class to its own file. +* Move UbloxTopicDiagnostic to its own file. +* Make gnss a member variable of UbloxNode. +* Move kNavSvInfoSubscribeRate into the base class that uses it. +* Remove kSubscribeRate. +* Move the kDefaultMeasRate to the class that needs it. +* Remove kROSQueueSize constant. +* Make rtcms a member variable of UbloxNode. +* Switch to a structure for RTCMs. +* Make freq_diag a member variable of UbloxNode. +* Make updater a member variable of UbloxNode. +* Make frame_id a member variable. +* Make config_on_startup_flag a member variable. +* Make meas_rate a member variable. +* Make nav_rate a class variable. +* Lots of small code updates throughout the GPS module. +* Get rid of global 'debug' variable. +* Move serialization into the ublox_serialization module. +* More rearrangement of header files to make a more sane structure. +* Move FTSProduct class into its own file. +* UbloxNode is not a component. +* Move ComponentInterface to its own file. +* Remove the one static ros::Subscriber. +* Remove last static advertiser. +* Switch to const references for std::string where possible. +* Switch to more idiomatic publisher initialization. +* Switch the one use of tf to tf2. +* Remove 'using namespace' uses. +* Switch to non-boost asio. +* Remove uses of 'new' throughout the codebase. +* Remove the last of boost from node.cpp +* Remove most of boost from node.cpp/.hpp. +* Remove a bunch of boost from node.cpp +* Remove more boost. +* Remove boost from worker.hpp +* Remove some uses of boost from async_worker.hpp +* Fully de-boostify callback.hpp +* Rearrange messages. +* Switch out boost time and mutex for std +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette, Mabel Zhang + 1.2.0 (2019-11-19) ------------------ * Add support for ZED-F9P new RELPOSNED message and provide heading output diff --git a/ublox_msgs/CHANGELOG.rst b/ublox_msgs/CHANGELOG.rst index a1a4ebfd..cf6591cc 100644 --- a/ublox_msgs/CHANGELOG.rst +++ b/ublox_msgs/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package ublox_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes to compile on Foxy. +* Fix heading output to comply with REP-103 +* Fixes pointed out by clang-tidy. +* Small style fixups. +* Port ublox_msgs to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Move serialization into the ublox_serialization module. +* More rearrangement of header files to make a more sane structure. +* Move the directory structure around just a bit. +* Rearrange messages. +* Remove boost from ublox_msgs.hpp serialization. +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ * Add support for ZED-F9P new RELPOSNED message and provide heading output diff --git a/ublox_serialization/CHANGELOG.rst b/ublox_serialization/CHANGELOG.rst index ff4e0431..bf32c894 100644 --- a/ublox_serialization/CHANGELOG.rst +++ b/ublox_serialization/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package ublox_serialization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Initial ROS 2 port to Dashing +* Fixes pointed out by clang-tidy. +* Make sure to initialize checksum properly. +* Port of ublox_serialization to ROS 2. +* Start ROS 2 port by COLCON_IGNORE everything. +* Remove unused vector serialization. +* Move serialization into the ublox_serialization module. +* Move the directory structure around just a bit. +* Remove boost from serialization. +* Remove trailing whitespace in all files. +* Rename header files to have .hpp extension. +* Contributors: Chao Qu, Chris Lalancette + 1.2.0 (2019-11-19) ------------------ From 5f2e7274397a77d036df286c310c7eeffc336240 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Oct 2020 18:42:55 +0000 Subject: [PATCH 119/152] 2.0.0 --- ublox/CHANGELOG.rst | 4 ++-- ublox/package.xml | 2 +- ublox_gps/CHANGELOG.rst | 4 ++-- ublox_gps/package.xml | 2 +- ublox_msgs/CHANGELOG.rst | 4 ++-- ublox_msgs/package.xml | 2 +- ublox_serialization/CHANGELOG.rst | 4 ++-- ublox_serialization/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ublox/CHANGELOG.rst b/ublox/CHANGELOG.rst index 8b11883f..c2da2c87 100644 --- a/ublox/CHANGELOG.rst +++ b/ublox/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-10-13) +------------------ * Initial ROS 2 port to Dashing * Port the ublox package to ROS 2. * Start ROS 2 port by COLCON_IGNORE everything. diff --git a/ublox/package.xml b/ublox/package.xml index 20b2e2c9..85b55a42 100644 --- a/ublox/package.xml +++ b/ublox/package.xml @@ -1,7 +1,7 @@ ublox - 1.2.0 + 2.0.0 Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. Johannes Meyer Veronica Lane diff --git a/ublox_gps/CHANGELOG.rst b/ublox_gps/CHANGELOG.rst index 565ec4a0..f163e4b8 100644 --- a/ublox_gps/CHANGELOG.rst +++ b/ublox_gps/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_gps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-10-13) +------------------ * Initial ROS 2 port to Dashing * Create publishers based on ROS parameters (`#1 `_) * Make sure to reset for survey-in mode. diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 6162a5cd..7c4b9bd2 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -1,7 +1,7 @@ ublox_gps - 1.2.0 + 2.0.0 Driver for u-blox GPS devices. diff --git a/ublox_msgs/CHANGELOG.rst b/ublox_msgs/CHANGELOG.rst index cf6591cc..ee389f75 100644 --- a/ublox_msgs/CHANGELOG.rst +++ b/ublox_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-10-13) +------------------ * Fixes to compile on Foxy. * Fix heading output to comply with REP-103 * Fixes pointed out by clang-tidy. diff --git a/ublox_msgs/package.xml b/ublox_msgs/package.xml index 62c61ac0..df7f1d97 100644 --- a/ublox_msgs/package.xml +++ b/ublox_msgs/package.xml @@ -2,7 +2,7 @@ ublox_msgs - 1.2.0 + 2.0.0 ublox_msgs contains raw messages for u-blox GNSS devices. diff --git a/ublox_serialization/CHANGELOG.rst b/ublox_serialization/CHANGELOG.rst index bf32c894..45830368 100644 --- a/ublox_serialization/CHANGELOG.rst +++ b/ublox_serialization/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_serialization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2020-10-13) +------------------ * Initial ROS 2 port to Dashing * Fixes pointed out by clang-tidy. * Make sure to initialize checksum properly. diff --git a/ublox_serialization/package.xml b/ublox_serialization/package.xml index f5af37b0..9d02caba 100644 --- a/ublox_serialization/package.xml +++ b/ublox_serialization/package.xml @@ -1,7 +1,7 @@ ublox_serialization - 1.2.0 + 2.0.0 ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. From b54031ea1bb558447ddb65cb2b90a47d1fb184b4 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 25 Jan 2021 21:38:58 +0900 Subject: [PATCH 120/152] Fix wrong variable name in launch (#120) Signed-off-by: wep21 --- ublox_gps/launch/ublox_gps_node-launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ublox_gps/launch/ublox_gps_node-launch.py b/ublox_gps/launch/ublox_gps_node-launch.py index 7f0da280..5df5d0f9 100644 --- a/ublox_gps/launch/ublox_gps_node-launch.py +++ b/ublox_gps/launch/ublox_gps_node-launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( - target_action=velodyne_driver_node, + target_action=ublox_gps_node, on_exit=[launch.actions.EmitEvent( event=launch.events.Shutdown())], )), From 141b6abbb4192d70b6438cc01918fcc513adfdca Mon Sep 17 00:00:00 2001 From: Davidson Daniel Rojas Cediel <39452483+dadaroce@users.noreply.github.com> Date: Tue, 23 Feb 2021 10:10:32 -0500 Subject: [PATCH 121/152] [FEAT]: add launch and config directories to 'intall' package to avoid wrong launch location (#125) --- ublox_gps/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 98b00186..4c64aad1 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -93,4 +93,9 @@ install(DIRECTORY include/ DESTINATION include ) +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + ament_package() From 32ea097945f27f939954b2d9077ec436ca3ea57a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 23 Feb 2021 15:13:31 +0000 Subject: [PATCH 122/152] Fix warnings in launch. Starting in Foxy, node_name, node_executable, node_plugin, and node_namespace are deprecated and replaced with name, executable, plugin, and namespace, respectively. Signed-off-by: Chris Lalancette --- ublox_gps/launch/ublox_gps_node-composed-launch.py | 10 +++++----- ublox_gps/launch/ublox_gps_node-launch.py | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ublox_gps/launch/ublox_gps_node-composed-launch.py b/ublox_gps/launch/ublox_gps_node-composed-launch.py index f0be0099..76c7f5aa 100644 --- a/ublox_gps/launch/ublox_gps_node-composed-launch.py +++ b/ublox_gps/launch/ublox_gps_node-composed-launch.py @@ -51,15 +51,15 @@ def generate_launch_description(): with open(param_config, 'r') as f: params = yaml.safe_load(f)['ublox_gps_node']['ros__parameters'] container = ComposableNodeContainer( - node_name='ublox_gps_container', - node_namespace='', + name='ublox_gps_container', + namespace='', package='rclcpp_components', - node_executable='component_container', + executable='component_container', composable_node_descriptions=[ ComposableNode( package='ublox_gps', - node_plugin='ublox_node::UbloxNode', - node_name='ublox_gps_node', + plugin='ublox_node::UbloxNode', + name='ublox_gps_node', parameters=[params]), ], output='both', diff --git a/ublox_gps/launch/ublox_gps_node-launch.py b/ublox_gps/launch/ublox_gps_node-launch.py index 5df5d0f9..9a8d9b2a 100644 --- a/ublox_gps/launch/ublox_gps_node-launch.py +++ b/ublox_gps/launch/ublox_gps_node-launch.py @@ -45,7 +45,7 @@ def generate_launch_description(): 'config') params = os.path.join(config_directory, 'c94_m8p_rover.yaml') ublox_gps_node = launch_ros.actions.Node(package='ublox_gps', - node_executable='ublox_gps_node', + executable='ublox_gps_node', output='both', parameters=[params]) From 67dcd26694caa6454b73cc7cc4bf1f630466920f Mon Sep 17 00:00:00 2001 From: CHAIWIT PHONKHEN <46931394+deepinbubblegum@users.noreply.github.com> Date: Wed, 19 May 2021 22:41:01 +0700 Subject: [PATCH 123/152] add Ublox ZED_F9P config (#131) * add Ublox ZED_F9P config Co-authored-by: Chris Lalancette --- ublox_gps/config/zed_f9p.yaml | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 ublox_gps/config/zed_f9p.yaml diff --git a/ublox_gps/config/zed_f9p.yaml b/ublox_gps/config/zed_f9p.yaml new file mode 100644 index 00000000..9fd45c79 --- /dev/null +++ b/ublox_gps/config/zed_f9p.yaml @@ -0,0 +1,26 @@ +# Configuration Settings for C94-M8P device +ublox_gps_node: + ros__parameters: + debug: 0 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: gps + uart1: + baudrate: 9600 + # TMODE3 Config + tmode3: 1 # Survey-In Mode + sv_in: + reset: True # True: disables and re-enables survey-in (resets) + # False: Disables survey-in only if TMODE3 is + # disabled + min_dur: 300 # Survey-In Minimum Duration [s] + acc_lim: 3.0 # Survey-In Accuracy Limit [m] + + inf: + all: true # Whether to display all INF messages in console + + publish: + all: true + aid: + hui: false + nav: + posecef: false From 7456c7bb29074fc4e3f3d537b9043bd4fb89e8f8 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Tue, 12 Oct 2021 09:52:51 -0400 Subject: [PATCH 124/152] Add UDP support (#140) --- README.md | 2 +- ublox_gps/include/ublox_gps/async_worker.hpp | 43 ++++++++++++++++++++ ublox_gps/include/ublox_gps/gps.hpp | 7 ++++ ublox_gps/include/ublox_gps/node.hpp | 11 ++++- ublox_gps/src/gps.cpp | 35 ++++++++++++++++ ublox_gps/src/node.cpp | 17 ++++++++ 6 files changed, 112 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ebb31e79..15abbe93 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ublox -The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP communication is also supported by the driver (untested). +The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP/UDP communication is also supported by the driver (untested). The driver was originally written by Johannes Meyer. Changes made later are detailed in the version history below. diff --git a/ublox_gps/include/ublox_gps/async_worker.hpp b/ublox_gps/include/ublox_gps/async_worker.hpp index 2185e78c..2315163e 100644 --- a/ublox_gps/include/ublox_gps/async_worker.hpp +++ b/ublox_gps/include/ublox_gps/async_worker.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include @@ -211,6 +212,29 @@ void AsyncWorker::doWrite() { out_.clear(); write_condition_.notify_all(); } +template <> +inline void AsyncWorker::doWrite() { + std::lock_guard lock(write_mutex_); + // Do nothing if out buffer is empty + if (out_.size() == 0) { + return; + } + // Write all the data in the out buffer + stream_->send(asio::buffer(out_.data(), out_.size())); + + if (debug_ >= 2) { + // Print the data that was sent + std::ostringstream oss; + for (std::vector::iterator it = out_.begin(); + it != out_.end(); ++it) { + oss << std::hex << static_cast(*it) << " "; + } + RCLCPP_DEBUG(logger_, "U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str()); + } + // Clear the buffer & unlock + out_.clear(); + write_condition_.notify_all(); +} template void AsyncWorker::doRead() { @@ -231,6 +255,25 @@ void AsyncWorker::doRead() { std::bind(&AsyncWorker::readEnd, this, std::placeholders::_1, std::placeholders::_2)); } +template <> +inline void AsyncWorker::doRead() { + std::lock_guard lock(read_mutex_); + if (in_.size() - in_buffer_size_ == 0) { + // In some circumstances, it is possible that there is no room left in the + // buffer. This can happen, for instance, if one of the UBlox messages + // has a value in the Length field that is much larger than this buffer + // can accomodate. We definitely don't want to ask for a 0-byte read (as + // we will get into an endless loop of asking for, and then receiving, + // 0 bytes), so we just throw away all of the data in the buffer. + in_buffer_size_ = 0; + } + + stream_->async_receive( + asio::buffer(in_.data() + in_buffer_size_, + in_.size() - in_buffer_size_), + std::bind(&AsyncWorker::readEnd, this, + std::placeholders::_1, std::placeholders::_2)); +} template void AsyncWorker::readEnd(const asio::error_code& error, diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 003aa31b..4604ea0a 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -102,6 +102,13 @@ class Gps final { */ void initializeTcp(const std::string & host, const std::string & port); + /** + * @brief Initialize UDP I/O. + * @param host the UDP host + * @param port the UDP port + */ + void initializeUdp(const std::string & host, const std::string & port); + /** * @brief Initialize the Serial I/O port. * @param port the device port address diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 4aeeb84f..4e23b9c0 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -82,7 +82,9 @@ class UbloxNode final : public rclcpp::Node { public: //! How long to wait during I/O reset [s] constexpr static int kResetWait = 10; - //! how often (in seconds) to call poll messages + //! How often (in seconds) to send keep-alive message + constexpr static double kKeepAlivePeriod = 10.0; + //! How often (in seconds) to call poll messages constexpr static double kPollDuration = 1.0; // Constants used for diagnostic frequency updater //! [s] 5Hz diagnostic period @@ -179,9 +181,13 @@ class UbloxNode final : public rclcpp::Node { void addProductInterface(const std::string & product_category, const std::string & ref_rov = ""); + /** + * @brief Poll version message from the U-Blox device to keep socket active. + */ + void keepAlive(); + /** * @brief Poll messages from the U-Blox device. - * @param event a timer indicating how often to poll the messages */ void pollMessages(); @@ -275,6 +281,7 @@ class UbloxNode final : public rclcpp::Node { //! Handles communication with the U-Blox Device std::shared_ptr gps_; + rclcpp::TimerBase::SharedPtr keep_alive_; rclcpp::TimerBase::SharedPtr poller_; }; diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 3d63f43d..e71377d8 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include @@ -253,6 +254,40 @@ void Gps::initializeTcp(const std::string & host, const std::string & port) { setWorker(std::make_shared>(socket, io_service, 8192, debug_, logger_)); } +void Gps::initializeUdp(const std::string & host, const std::string & port) { + host_ = host; + port_ = port; + auto io_service = std::make_shared(); + asio::ip::udp::resolver::iterator endpoint; + + try { + asio::ip::udp::resolver resolver(*io_service); + endpoint = + resolver.resolve(asio::ip::udp::resolver::query(host, port)); + } catch (const std::runtime_error& e) { + throw std::runtime_error("U-Blox: Could not resolve" + host + " " + + port + " " + e.what()); + } + + auto socket = std::make_shared(*io_service); + + try { + socket->connect(*endpoint); + } catch (const std::runtime_error& e) { + throw std::runtime_error("U-Blox: Could not connect to " + + endpoint->host_name() + ":" + + endpoint->service_name() + ": " + e.what()); + } + + RCLCPP_INFO(logger_, "U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(), + endpoint->service_name().c_str()); + + if (worker_) { + return; + } + setWorker(std::make_shared>(socket, io_service, 8192, debug_, logger_)); +} + void Gps::close() { if (save_on_shutdown_) { if (saveOnShutdown()) { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 907de3c9..c6a5d5fd 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -482,6 +482,11 @@ void UbloxNode::getRosParams() { } } +void UbloxNode::keepAlive() { + // Poll version message to keep UDP socket active + gps_->poll(ublox_msgs::Class::MON, ublox_msgs::Message::MON::VER); +} + void UbloxNode::pollMessages() { static std::vector payload(1, 1); if (getRosBoolean(this, "publish.aid.alm")) { @@ -802,6 +807,12 @@ void UbloxNode::initializeIo() { RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps_->initializeTcp(host, port); + } else if (proto == "udp") { + std::string host(match[2]); + std::string port(match[3]); + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), + port.c_str()); + gps_->initializeUdp(host, port); } else { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } @@ -851,6 +862,12 @@ void UbloxNode::initialize() { // Configure INF messages (needs INF params, call after subscribing) configureInf(); + if (device_.substr(0, 6) == "udp://") { + // Setup timer to poll version message to keep UDP socket active + keep_alive_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(kKeepAlivePeriod * 1000.0)), + std::bind(&UbloxNode::keepAlive, this)); + } + poller_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(kPollDuration * 1000.0)), std::bind(&UbloxNode::pollMessages, this)); } From daaa065af6290edbaaf0055220f57bb4e607002c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 15 Oct 2021 09:32:09 -0400 Subject: [PATCH 125/152] Add the types to declared parameters. (#141) This removes warnings when building on Galactic. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index c6a5d5fd..fc0ea0b2 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -250,8 +250,8 @@ void UbloxNode::getRosParams() { uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - this->declare_parameter("usb.in"); - this->declare_parameter("usb.out"); + this->declare_parameter("usb.in"); + this->declare_parameter("usb.out"); usb_tx_ = declareRosIntParameter(this, "usb.tx_ready", 0); if (isRosParameterSet(this, "usb.in") || isRosParameterSet(this, "usb.out")) { set_usb_ = true; @@ -271,8 +271,8 @@ void UbloxNode::getRosParams() { nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles // RTCM params - this->declare_parameter("rtcm.ids"); - this->declare_parameter("rtcm.rates"); + this->declare_parameter>("rtcm.ids"); + this->declare_parameter>("rtcm.rates"); std::vector rtcm_ids; std::vector rtcm_rates; this->get_parameter("rtcm.ids", rtcm_ids); @@ -319,11 +319,11 @@ void UbloxNode::getRosParams() { this->declare_parameter("dat.set", false); - this->declare_parameter("dat.majA"); - this->declare_parameter("dat.flat"); - this->declare_parameter("dat.shift"); - this->declare_parameter("dat.rot"); - this->declare_parameter("dat.scale"); + this->declare_parameter("dat.majA"); + this->declare_parameter("dat.flat"); + this->declare_parameter>("dat.shift"); + this->declare_parameter>("dat.rot"); + this->declare_parameter("dat.scale"); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) @@ -368,7 +368,7 @@ void UbloxNode::getRosParams() { this->declare_parameter("sv_in.min_dur", 0); this->declare_parameter("sv_in.acc_lim", 0.0); - this->declare_parameter("dgnss_mode"); + this->declare_parameter("dgnss_mode"); // raw data stream logging this->declare_parameter("raw_data_stream.enable", false); @@ -450,9 +450,9 @@ void UbloxNode::getRosParams() { // HNR parameters this->declare_parameter("publish.hnr.pvt", true); - this->declare_parameter("tmode3"); - this->declare_parameter("arp.position"); - this->declare_parameter("arp.position_hp"); + this->declare_parameter("tmode3"); + this->declare_parameter>("arp.position"); + this->declare_parameter>("arp.position_hp"); this->declare_parameter("arp.acc", 0.0); this->declare_parameter("arp.lla_flag", false); From 24c4722e3579cba3dd34b73f132f8717da4beea7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 4 Nov 2021 11:07:14 -0400 Subject: [PATCH 126/152] Fix parameter declaration types. (#146) Instead of trying to force things with template arguments, instead explicitly declare the type when declaring arguments without a default. This should allow the types to be properly set while still allowing these to be run without a parameter explicitly set. Signed-off-by: Chris Lalancette --- ublox_gps/src/node.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index fc0ea0b2..23f51727 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -250,8 +250,8 @@ void UbloxNode::getRosParams() { uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; - this->declare_parameter("usb.in"); - this->declare_parameter("usb.out"); + this->declare_parameter("usb.in", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("usb.out", rclcpp::PARAMETER_INTEGER); usb_tx_ = declareRosIntParameter(this, "usb.tx_ready", 0); if (isRosParameterSet(this, "usb.in") || isRosParameterSet(this, "usb.out")) { set_usb_ = true; @@ -271,8 +271,8 @@ void UbloxNode::getRosParams() { nav_rate_ = declareRosIntParameter(this, "nav_rate", 1); // # of measurement rate cycles // RTCM params - this->declare_parameter>("rtcm.ids"); - this->declare_parameter>("rtcm.rates"); + this->declare_parameter("rtcm.ids", rclcpp::PARAMETER_INTEGER_ARRAY); + this->declare_parameter("rtcm.rates", rclcpp::PARAMETER_INTEGER_ARRAY); std::vector rtcm_ids; std::vector rtcm_rates; this->get_parameter("rtcm.ids", rtcm_ids); @@ -319,11 +319,11 @@ void UbloxNode::getRosParams() { this->declare_parameter("dat.set", false); - this->declare_parameter("dat.majA"); - this->declare_parameter("dat.flat"); - this->declare_parameter>("dat.shift"); - this->declare_parameter>("dat.rot"); - this->declare_parameter("dat.scale"); + this->declare_parameter("dat.majA", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("dat.flat", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("dat.shift", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("dat.rot", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("dat.scale", rclcpp::PARAMETER_DOUBLE); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) @@ -368,7 +368,7 @@ void UbloxNode::getRosParams() { this->declare_parameter("sv_in.min_dur", 0); this->declare_parameter("sv_in.acc_lim", 0.0); - this->declare_parameter("dgnss_mode"); + this->declare_parameter("dgnss_mode", rclcpp::PARAMETER_INTEGER); // raw data stream logging this->declare_parameter("raw_data_stream.enable", false); @@ -450,9 +450,9 @@ void UbloxNode::getRosParams() { // HNR parameters this->declare_parameter("publish.hnr.pvt", true); - this->declare_parameter("tmode3"); - this->declare_parameter>("arp.position"); - this->declare_parameter>("arp.position_hp"); + this->declare_parameter("tmode3", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("arp.position", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("arp.position_hp", rclcpp::PARAMETER_INTEGER_ARRAY); this->declare_parameter("arp.acc", 0.0); this->declare_parameter("arp.lla_flag", false); From 47b9df70ca4ff7b6606ee3e482142d88d71068e0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 13 Apr 2022 12:41:58 +0000 Subject: [PATCH 127/152] Revamp the building of the driver for modern ROS 2 practices. This includes getting rid of ament_target_dependencies in favor of target_link_libraries, installing includes to another level down in the hierarchy, and generally making things use modern CMake. Note that all of this only applies to ROS 2 Humble and later. Signed-off-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 58 ++++++++++++++++-------------- ublox_msgs/CMakeLists.txt | 40 ++++++++++++--------- ublox_serialization/CMakeLists.txt | 12 +++++-- 3 files changed, 65 insertions(+), 45 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 4c64aad1..5a9fdf1f 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -26,8 +26,6 @@ find_package(tf2 REQUIRED) find_package(ublox_msgs REQUIRED) find_package(ublox_serialization REQUIRED) -include_directories(include) - # build node add_library(ublox_gps src/adr_udr_product.cpp @@ -46,43 +44,48 @@ add_library(ublox_gps src/ublox_firmware7.cpp src/ublox_firmware8.cpp src/ublox_firmware9.cpp) -ament_target_dependencies(ublox_gps - "asio" - "diagnostic_msgs" - "diagnostic_updater" - "geometry_msgs" - "rcl_interfaces" - "rclcpp" - "rclcpp_components" - "sensor_msgs" - "std_msgs" - "tf2" - "ublox_msgs" - "ublox_serialization" +target_include_directories(ublox_gps PUBLIC + "$" + "$" + ${diagnostic_updater_INCLUDE_DIRS} +) +target_link_libraries(ublox_gps PUBLIC + ${asio_LIBRARIES} + ${diagnostic_updater_LIBRARIES} + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_components::component + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + tf2::tf2 + ${ublox_msgs_TARGETS} + ublox_serialization::ublox_serialization ) -install(TARGETS ublox_gps +install(TARGETS ublox_gps EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) add_executable(ublox_gps_node src/node_main.cpp) -ament_target_dependencies(ublox_gps_node - "rclcpp" +target_link_libraries(ublox_gps_node PRIVATE + rclcpp::rclcpp + ublox_gps ) -target_link_libraries(ublox_gps_node ublox_gps) # build logger node -add_executable(ublox_logger_node src/logger_node_pa.cpp src/raw_data_pa.cpp) -set_target_properties(ublox_logger_node PROPERTIES OUTPUT_NAME ublox_logger) -ament_target_dependencies(ublox_logger_node - "rclcpp" - "std_msgs" +add_executable(ublox_logger src/logger_node_pa.cpp src/raw_data_pa.cpp) +target_link_libraries(ublox_logger PRIVATE + rclcpp::rclcpp + ${std_msgs_TARGETS} + ublox_gps ) install(TARGETS - ublox_gps_node ublox_logger_node + ublox_gps_node ublox_logger DESTINATION lib/${PROJECT_NAME} ) @@ -90,7 +93,7 @@ rclcpp_components_register_nodes(ublox_gps "ublox_node::UbloxNode") install(DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) install( @@ -98,4 +101,7 @@ install( DESTINATION share/${PROJECT_NAME} ) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME}) + ament_package() diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 35fdf76f..e4c03c01 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -103,26 +103,32 @@ rosidl_generate_interfaces(${PROJECT_NAME} std_msgs ) -include_directories(include) -add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) -rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") -ament_target_dependencies(${PROJECT_NAME}_lib - "ublox_serialization" -) +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") +if(cpp_typesupport_target) + add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) + target_include_directories(${PROJECT_NAME}_lib PRIVATE + "$" + "$") + target_link_libraries(${PROJECT_NAME}_lib + ${cpp_typesupport_target} + ublox_serialization::ublox_serialization + ) -install(TARGETS ${PROJECT_NAME}_lib EXPORT ${PROJECT_NAME}_lib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + install(TARGETS ${PROJECT_NAME}_lib EXPORT ${PROJECT_NAME}_lib + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) -install(DIRECTORY include/ - DESTINATION include -) + install(DIRECTORY include/ + DESTINATION "include/${PROJECT_NAME}" + ) + + ament_export_include_directories("include/${PROJECT_NAME}") + ament_export_libraries(${PROJECT_NAME}_lib) + ament_export_targets(${PROJECT_NAME}_lib) +endif() -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}_lib) -ament_export_targets(${PROJECT_NAME}_lib) ament_export_dependencies(rosidl_default_runtime sensor_msgs std_msgs ublox_serialization) ament_package() diff --git a/ublox_serialization/CMakeLists.txt b/ublox_serialization/CMakeLists.txt index ca3bf732..15bb03a4 100644 --- a/ublox_serialization/CMakeLists.txt +++ b/ublox_serialization/CMakeLists.txt @@ -4,10 +4,18 @@ project(ublox_serialization) find_package(ament_cmake REQUIRED) +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories(${PROJECT_NAME} INTERFACE + "$" + "$") + install(DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME}) ament_package() From 31d2cd4c053da347de98f2e8bec696197f203f67 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 13 Apr 2022 13:26:10 +0000 Subject: [PATCH 128/152] Changelog. Signed-off-by: Chris Lalancette --- ublox/CHANGELOG.rst | 3 +++ ublox_gps/CHANGELOG.rst | 12 ++++++++++++ ublox_msgs/CHANGELOG.rst | 5 +++++ ublox_serialization/CHANGELOG.rst | 5 +++++ 4 files changed, 25 insertions(+) diff --git a/ublox/CHANGELOG.rst b/ublox/CHANGELOG.rst index c2da2c87..dd9485ff 100644 --- a/ublox/CHANGELOG.rst +++ b/ublox/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ublox ^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2020-10-13) ------------------ * Initial ROS 2 port to Dashing diff --git a/ublox_gps/CHANGELOG.rst b/ublox_gps/CHANGELOG.rst index f163e4b8..4a4adc69 100644 --- a/ublox_gps/CHANGELOG.rst +++ b/ublox_gps/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ublox_gps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revamp the building of the driver for modern ROS 2 practices. +* Fix parameter declaration types. (`#146 `_) +* Add the types to declared parameters. (`#141 `_) +* Add UDP support (`#140 `_) +* add Ublox ZED_F9P config (`#131 `_) +* Fix warnings in launch. +* [FEAT]: add launch and config directories to 'intall' package to avoid wrong launch location (`#125 `_) +* Fix wrong variable name in launch (`#120 `_) +* Contributors: CHAIWIT PHONKHEN, Chao Qu, Chris Lalancette, Daisuke Nishimatsu, Davidson Daniel Rojas Cediel, Kevin Hallenbeck + 2.0.0 (2020-10-13) ------------------ * Initial ROS 2 port to Dashing diff --git a/ublox_msgs/CHANGELOG.rst b/ublox_msgs/CHANGELOG.rst index ee389f75..4224c58a 100644 --- a/ublox_msgs/CHANGELOG.rst +++ b/ublox_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ublox_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revamp the building of the driver for modern ROS 2 practices. +* Contributors: Chao Qu, Chris Lalancette + 2.0.0 (2020-10-13) ------------------ * Fixes to compile on Foxy. diff --git a/ublox_serialization/CHANGELOG.rst b/ublox_serialization/CHANGELOG.rst index 45830368..f8a23af3 100644 --- a/ublox_serialization/CHANGELOG.rst +++ b/ublox_serialization/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ublox_serialization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revamp the building of the driver for modern ROS 2 practices. +* Contributors: Chao Qu, Chris Lalancette + 2.0.0 (2020-10-13) ------------------ * Initial ROS 2 port to Dashing From b6187dd50f5f91bf266987e67365ecc2c59ce60d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 13 Apr 2022 13:42:51 +0000 Subject: [PATCH 129/152] 2.3.0 --- ublox/CHANGELOG.rst | 4 ++-- ublox/package.xml | 2 +- ublox_gps/CHANGELOG.rst | 4 ++-- ublox_gps/package.xml | 2 +- ublox_msgs/CHANGELOG.rst | 4 ++-- ublox_msgs/package.xml | 2 +- ublox_serialization/CHANGELOG.rst | 4 ++-- ublox_serialization/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ublox/CHANGELOG.rst b/ublox/CHANGELOG.rst index dd9485ff..c6570749 100644 --- a/ublox/CHANGELOG.rst +++ b/ublox/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-13) +------------------ 2.0.0 (2020-10-13) ------------------ diff --git a/ublox/package.xml b/ublox/package.xml index 85b55a42..b34a1235 100644 --- a/ublox/package.xml +++ b/ublox/package.xml @@ -1,7 +1,7 @@ ublox - 2.0.0 + 2.3.0 Provides a ublox_gps node for u-blox GPS receivers, messages, and serialization packages for the binary UBX protocol. Johannes Meyer Veronica Lane diff --git a/ublox_gps/CHANGELOG.rst b/ublox_gps/CHANGELOG.rst index 4a4adc69..e5f4cc88 100644 --- a/ublox_gps/CHANGELOG.rst +++ b/ublox_gps/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_gps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-13) +------------------ * Revamp the building of the driver for modern ROS 2 practices. * Fix parameter declaration types. (`#146 `_) * Add the types to declared parameters. (`#141 `_) diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 7c4b9bd2..094aa590 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -1,7 +1,7 @@ ublox_gps - 2.0.0 + 2.3.0 Driver for u-blox GPS devices. diff --git a/ublox_msgs/CHANGELOG.rst b/ublox_msgs/CHANGELOG.rst index 4224c58a..b61a7f98 100644 --- a/ublox_msgs/CHANGELOG.rst +++ b/ublox_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-13) +------------------ * Revamp the building of the driver for modern ROS 2 practices. * Contributors: Chao Qu, Chris Lalancette diff --git a/ublox_msgs/package.xml b/ublox_msgs/package.xml index df7f1d97..35da63ff 100644 --- a/ublox_msgs/package.xml +++ b/ublox_msgs/package.xml @@ -2,7 +2,7 @@ ublox_msgs - 2.0.0 + 2.3.0 ublox_msgs contains raw messages for u-blox GNSS devices. diff --git a/ublox_serialization/CHANGELOG.rst b/ublox_serialization/CHANGELOG.rst index f8a23af3..b399ea27 100644 --- a/ublox_serialization/CHANGELOG.rst +++ b/ublox_serialization/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ublox_serialization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-13) +------------------ * Revamp the building of the driver for modern ROS 2 practices. * Contributors: Chao Qu, Chris Lalancette diff --git a/ublox_serialization/package.xml b/ublox_serialization/package.xml index 9d02caba..6492dcf4 100644 --- a/ublox_serialization/package.xml +++ b/ublox_serialization/package.xml @@ -1,7 +1,7 @@ ublox_serialization - 2.0.0 + 2.3.0 ublox_serialization provides header files for serialization of ROS messages to and from u-blox message format. From 1b126697272fe7789f818bf4594384470ccac34d Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 14 Jun 2022 00:43:20 +0900 Subject: [PATCH 130/152] Fix topic name (#9) (#163) Signed-off-by: wep21 Signed-off-by: Takayuki AKAMINE --- ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp | 6 +++--- ublox_gps/src/ublox_firmware6.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp index 84ffe7a7..ed3ab4b2 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -35,13 +35,13 @@ class UbloxFirmware7Plus : public UbloxFirmware { : UbloxFirmware(updater, gnss, node), frame_id_(frame_id), freq_diag_(freq_diag) { // NavPVT publisher if (getRosBoolean(node_, "publish.nav.pvt")) { - nav_pvt_pub_ = node_->create_publisher("navpvt", 1); + nav_pvt_pub_ = node_->create_publisher("~/navpvt", 1); } fix_pub_ = - node_->create_publisher("fix", 1); + node_->create_publisher("~/fix", 1); vel_pub_ = - node_->create_publisher("fix_velocity", + node_->create_publisher("~/fix_velocity", 1); } diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index 0bb7f3b5..0a0dbe66 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -36,7 +36,7 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrcreate_publisher("fix", 1); + node_->create_publisher("~/fix", 1); if (getRosBoolean(node_, "publish.nav.velned")) { nav_vel_ned_pub_ = @@ -44,7 +44,7 @@ UbloxFirmware6::UbloxFirmware6(const std::string & frame_id, std::shared_ptrcreate_publisher("fix_velocity", + node_->create_publisher("~/fix_velocity", 1); if (getRosBoolean(node_, "publish.nav.sol")) { From 3c1784ba805b0c6f8815f51de338d2df7ac38ab6 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 14 Jun 2022 00:43:56 +0900 Subject: [PATCH 131/152] Remove periodical force update (#12) (#161) Signed-off-by: wep21 --- ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp | 1 - ublox_gps/src/adr_udr_product.cpp | 1 - ublox_gps/src/hp_pos_rec_product.cpp | 1 - ublox_gps/src/hpg_ref_product.cpp | 1 - ublox_gps/src/hpg_rov_product.cpp | 1 - ublox_gps/src/tim_product.cpp | 1 - ublox_gps/src/ublox_firmware6.cpp | 1 - 7 files changed, 7 deletions(-) diff --git a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp index ed3ab4b2..db4a2ca5 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware7plus.hpp @@ -138,7 +138,6 @@ class UbloxFirmware7Plus : public UbloxFirmware { // last_nav_pvt_ = m; freq_diag_->diagnostic->tick(fix.header.stamp); - updater_->force_update(); } protected: diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 3a19c172..84926c2d 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -202,7 +202,6 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { } } - updater_->force_update(); } } // namespace ublox_node diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index 3e0a44ca..e89af884 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -74,7 +74,6 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 } last_rel_pos_ = m; - updater_->force_update(); } } // namespace ublox_node diff --git a/ublox_gps/src/hpg_ref_product.cpp b/ublox_gps/src/hpg_ref_product.cpp index b12caaff..5fef3573 100644 --- a/ublox_gps/src/hpg_ref_product.cpp +++ b/ublox_gps/src/hpg_ref_product.cpp @@ -188,7 +188,6 @@ void HpgRefProduct::callbackNavSvIn(const ublox_msgs::msg::NavSVIN& m) { setTimeMode(gps_); } - updater_->force_update(); } bool HpgRefProduct::setTimeMode(std::shared_ptr gps) { diff --git a/ublox_gps/src/hpg_rov_product.cpp b/ublox_gps/src/hpg_rov_product.cpp index 7b524d57..76baef27 100644 --- a/ublox_gps/src/hpg_rov_product.cpp +++ b/ublox_gps/src/hpg_rov_product.cpp @@ -98,7 +98,6 @@ void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED &m) } last_rel_pos_ = m; - updater_->force_update(); } } // namespace ublox_node diff --git a/ublox_gps/src/tim_product.cpp b/ublox_gps/src/tim_product.cpp index 35f1a9cd..3f9000f1 100644 --- a/ublox_gps/src/tim_product.cpp +++ b/ublox_gps/src/tim_product.cpp @@ -90,7 +90,6 @@ void TimProduct::callbackTimTM2(const ublox_msgs::msg::TimTM2 &m) { interrupt_time_pub_->publish(t_ref_); } - updater_->force_update(); } void TimProduct::initializeRosDiagnostics() { diff --git a/ublox_gps/src/ublox_firmware6.cpp b/ublox_gps/src/ublox_firmware6.cpp index 0a0dbe66..65922b82 100644 --- a/ublox_gps/src/ublox_firmware6.cpp +++ b/ublox_gps/src/ublox_firmware6.cpp @@ -204,7 +204,6 @@ void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::msg::NavPOSLLH& m) { last_nav_pos_ = m; // update diagnostics freq_diag_->diagnostic->tick(fix_.header.stamp); - updater_->force_update(); } void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::msg::NavVELNED& m) { From 56a3a0bb19e6ad2f5bfd35259a8ac61bcebd3c23 Mon Sep 17 00:00:00 2001 From: PhilippPolterauer <45892981+PhilippPolterauer@users.noreply.github.com> Date: Wed, 7 Sep 2022 19:32:13 +0200 Subject: [PATCH 132/152] Fix warning for unsupported SPG mode (#181) Co-authored-by: Philipp Polterauer --- ublox_gps/src/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 23f51727..3e1079be 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -225,10 +225,10 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); } else if (product_category == "FTS") { components_.push_back(std::make_shared()); - } else if (product_category == "SPG") { + } else { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), - "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, SPG"); + "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS"); } } From 7c77ebc21fbc967a3d8c04fad461a616c82dc71e Mon Sep 17 00:00:00 2001 From: PhilippPolterauer <45892981+PhilippPolterauer@users.noreply.github.com> Date: Thu, 8 Sep 2022 23:20:09 +0200 Subject: [PATCH 133/152] HPS Device Support ( C102-f9r) (#182) * implemented support for HPS c102-f9r Co-authored-by: Philipp Polterauer --- ublox_gps/src/node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 3e1079be..6931d427 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -225,10 +225,13 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); } else if (product_category == "FTS") { components_.push_back(std::make_shared()); + } else if (product_category == "HPS") { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(nav_rate_, updater_, this)); } else { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), - "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS"); + "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, HPS"); } } From 71e2f0c7562c5c97c72045a7d6758447535c8a3d Mon Sep 17 00:00:00 2001 From: "Ryan D. Lewis" Date: Tue, 18 Oct 2022 11:01:49 -0500 Subject: [PATCH 134/152] Add support for forwarding RTCM correction data (#169) --- ublox_gps/CMakeLists.txt | 2 ++ ublox_gps/include/ublox_gps/gps.hpp | 6 ++++++ ublox_gps/include/ublox_gps/node.hpp | 11 +++++++++++ ublox_gps/package.xml | 1 + ublox_gps/src/gps.cpp | 5 +++++ ublox_gps/src/node.cpp | 7 +++++++ 6 files changed, 32 insertions(+) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 5a9fdf1f..a3a7b86a 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(ublox_msgs REQUIRED) find_package(ublox_serialization REQUIRED) +find_package(rtcm_msgs REQUIRED) # build node add_library(ublox_gps @@ -57,6 +58,7 @@ target_link_libraries(ublox_gps PUBLIC ${rcl_interfaces_TARGETS} rclcpp::rclcpp rclcpp_components::component + ${rtcm_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} tf2::tf2 diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 4604ea0a..59070409 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -128,6 +128,12 @@ class Gps final { */ void resetSerial(const std::string & port); + /** + * @brief Send rtcm correction messages to the connected device. + * @param message the RTCM correction data as a vector + */ + bool sendRtcm(const std::vector &message); + /** * @brief Closes the I/O port, and initiates save on shutdown procedure * if enabled. diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 4e23b9c0..68b2b717 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -41,6 +41,7 @@ #include #include #include +#include // Ublox GPS includes #include #include @@ -136,6 +137,16 @@ class UbloxNode final : public rclcpp::Node { private: + /** + * @brief Callback for '/ntrip_client/rtcm' subscription to handle RTCM correction data + */ + void rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg); + + /** + * @brief Subscription handler for RTCM data + */ + rclcpp::Subscription::SharedPtr subscription_; + /** * @brief Initialize the I/O handling. */ diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 094aa590..4321fc93 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -26,6 +26,7 @@ tf2 ublox_msgs ublox_serialization + rtcm_msgs ament_cmake diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index e71377d8..806dc844 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -576,6 +576,11 @@ bool Gps::setUseAdr(bool enable) { return configure(msg); } +bool Gps::sendRtcm(const std::vector& rtcm) { + worker_->send(rtcm.data(), rtcm.size()); + return true; +} + bool Gps::poll(uint8_t class_id, uint8_t message_id, const std::vector& payload) { if (!worker_) { diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 6931d427..49e837b2 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -190,6 +190,10 @@ UbloxNode::UbloxNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ublox_ initialize(); } +void UbloxNode::rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg) { + gps_->sendRtcm(msg->message); +} + void UbloxNode::addFirmwareInterface() { int ublox_version; if (protocol_version_ < 14.0) { @@ -483,6 +487,9 @@ void UbloxNode::getRosParams() { if (getRosBoolean(this, "publish.aid.hui")) { aid_hui_pub_ = this->create_publisher("aidhui", 1); } + + // Create subscriber for RTCM correction data to enable RTK + this->subscription_ = this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); } void UbloxNode::keepAlive() { From 88673b780c9862d3944fb105cbfde4ce067fa29d Mon Sep 17 00:00:00 2001 From: kagibson Date: Wed, 11 Jan 2023 06:26:37 -0800 Subject: [PATCH 135/152] Add position and velocity covariance message (UBX_NAV_COV) (#196) * Add ubx-nav-cov message type * Add navcov message * Subscribe to message * Add serializer --- ublox_gps/include/ublox_gps/node.hpp | 1 + ublox_gps/src/node.cpp | 11 +++- ublox_msgs/CMakeLists.txt | 1 + .../include/ublox_msgs/serialization.hpp | 50 +++++++++++++++++++ ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 2 + ublox_msgs/msg/NavCOV.msg | 28 +++++++++++ ublox_msgs/src/ublox_msgs.cpp | 2 + 7 files changed, 93 insertions(+), 2 deletions(-) create mode 100644 ublox_msgs/msg/NavCOV.msg diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 68b2b717..20852893 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -265,6 +265,7 @@ class UbloxNode final : public rclcpp::Node { rclcpp::Publisher::SharedPtr nav_status_pub_; rclcpp::Publisher::SharedPtr nav_posecef_pub_; rclcpp::Publisher::SharedPtr nav_clock_pub_; + rclcpp::Publisher::SharedPtr nav_cov_pub_; rclcpp::Publisher::SharedPtr aid_alm_pub_; rclcpp::Publisher::SharedPtr aid_eph_pub_; rclcpp::Publisher::SharedPtr aid_hui_pub_; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 49e837b2..2336b193 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -410,6 +411,7 @@ void UbloxNode::getRosParams() { this->declare_parameter("publish.nav.all", getRosBoolean(this, "publish.all")); this->declare_parameter("publish.nav.att", getRosBoolean(this, "publish.nav.all")); this->declare_parameter("publish.nav.clock", getRosBoolean(this, "publish.nav.all")); + this->declare_parameter("publish.nav.cov", getRosBoolean(this, "publish.nav.all")); this->declare_parameter("publish.nav.heading", getRosBoolean(this, "publish.nav.all")); this->declare_parameter("publish.nav.posecef", getRosBoolean(this, "publish.nav.all")); this->declare_parameter("publish.nav.posllh", getRosBoolean(this, "publish.nav.all")); @@ -472,8 +474,8 @@ void UbloxNode::getRosParams() { if (getRosBoolean(this, "publish.nav.posecef")) { nav_posecef_pub_ = this->create_publisher("navposecef", 1); } - if (getRosBoolean(this, "publish.nav.clock")) { - nav_clock_pub_ = this->create_publisher("navclock", 1); + if (getRosBoolean(this, "publish.nav.cov")) { + nav_cov_pub_ = this->create_publisher("navcov", 1); } if (getRosBoolean(this, "publish.nav.clock")) { nav_clock_pub_ = this->create_publisher("navclock", 1); @@ -547,6 +549,11 @@ void UbloxNode::subscribe() { 1); } + if (getRosBoolean(this, "publish.nav.cov")) { + gps_->subscribe([this](const ublox_msgs::msg::NavCOV &m) { nav_cov_pub_->publish(m); }, + 1); + } + // INF messages if (getRosBoolean(this, "inf.debug")) { gps_->subscribeId( diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index e4c03c01..122cf216 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -58,6 +58,7 @@ set(msg_files "msg/MonVER.msg" "msg/NavATT.msg" "msg/NavCLOCK.msg" + "msg/NavCOV.msg" "msg/NavDGPS.msg" "msg/NavDGPSSV.msg" "msg/NavDOP.msg" diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 9ef607fc..89046e74 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1504,6 +1504,56 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavCOV_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.pos_cov_valid); + stream.next(m.vel_cov_valid); + stream.next(m.reserved_0); + stream.next(m.pos_cov_nn); + stream.next(m.pos_cov_ne); + stream.next(m.pos_cov_nd); + stream.next(m.pos_cov_ee); + stream.next(m.pos_cov_ed); + stream.next(m.pos_cov_dd); + stream.next(m.vel_cov_nn); + stream.next(m.vel_cov_ne); + stream.next(m.vel_cov_nd); + stream.next(m.vel_cov_ee); + stream.next(m.vel_cov_ed); + stream.next(m.vel_cov_dd); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavCOV_ & m) { + (void)m; + return 64; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavCOV_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.pos_cov_valid); + stream.next(m.vel_cov_valid); + stream.next(m.reserved_0); + stream.next(m.pos_cov_nn); + stream.next(m.pos_cov_ne); + stream.next(m.pos_cov_nd); + stream.next(m.pos_cov_ee); + stream.next(m.pos_cov_ed); + stream.next(m.pos_cov_dd); + stream.next(m.vel_cov_nn); + stream.next(m.vel_cov_ne); + stream.next(m.vel_cov_nd); + stream.next(m.vel_cov_ee); + stream.next(m.vel_cov_ed); + stream.next(m.vel_cov_dd); + } +}; + template struct UbloxSerializer > { inline static void read(UbloxIStream& stream, ublox_msgs::msg::NavDGPSSV_ & m) { diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index d362c8d0..2559a7b8 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -154,6 +155,7 @@ namespace Message { namespace NAV { static const uint8_t ATT = ublox_msgs::msg::NavATT::MESSAGE_ID; static const uint8_t CLOCK = ublox_msgs::msg::NavCLOCK::MESSAGE_ID; + static const uint8_t COV = ublox_msgs::msg::NavCOV::MESSAGE_ID; static const uint8_t DGPS = ublox_msgs::msg::NavDGPS::MESSAGE_ID; static const uint8_t DOP = ublox_msgs::msg::NavDOP::MESSAGE_ID; static const uint8_t POSECEF = ublox_msgs::msg::NavPOSECEF::MESSAGE_ID; diff --git a/ublox_msgs/msg/NavCOV.msg b/ublox_msgs/msg/NavCOV.msg new file mode 100644 index 00000000..d70d7be2 --- /dev/null +++ b/ublox_msgs/msg/NavCOV.msg @@ -0,0 +1,28 @@ +# NAV-COV (0x01 0x36) +# Covariance matrices for position and velocity solutions +# + +uint8 CLASS_ID = 1 +uint8 MESSAGE_ID = 54 + +uint32 i_tow # GPS Millisecond time of week [ms] + +uint8 version # Message version (0x00 for this version) +uint8 pos_cov_valid # Position covariance matrix validity flag +uint8 vel_cov_valid # Velocity covariance matrix validity flag +uint8[9] reserved_0 # Reserved + +float32 pos_cov_nn # Position covariance matrix value p_NN [m^2] +float32 pos_cov_ne # Position covariance matrix value p_NE [m^2] +float32 pos_cov_nd # Position covariance matrix value p_ND [m^2] +float32 pos_cov_ee # Position covariance matrix value p_EE [m^2] +float32 pos_cov_ed # Position covariance matrix value p_ED [m^2] +float32 pos_cov_dd # Position covariance matrix value p_DD [m^2] + + +float32 vel_cov_nn # Velocity covariance matrix value v_NN [m^2/s^2] +float32 vel_cov_ne # Velocity covariance matrix value v_NE [m^2/s^2] +float32 vel_cov_nd # Velocity covariance matrix value v_ND [m^2/s^2] +float32 vel_cov_ee # Velocity covariance matrix value v_EE [m^2/s^2] +float32 vel_cov_ed # Velocity covariance matrix value v_ED [m^2/s^2] +float32 vel_cov_dd # Velocity covariance matrix value v_DD [m^2/s^2] diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index cb2f47d1..3b9c7f73 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -38,6 +38,8 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, ublox_msgs, NavATT) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::CLOCK, ublox_msgs, NavCLOCK) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::COV, + ublox_msgs, NavCOV) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DGPS, ublox_msgs, NavDGPS) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::DOP, From b6e02704eb3aa6270b8926b64b5fc5822503199e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 2 Feb 2023 16:01:02 -0500 Subject: [PATCH 136/152] Bugfix: IMU gyro unit fix to comply with sensor_msgs definition (#191) The sensor_msgs/Imu.msgs requires angular velocity readings of the IMU to be published in rad/s. This PR changes the unit from deg/s to rad/s to comply with the message definition. Signed-off-by: Chris Lalancette --- ublox_gps/src/adr_udr_product.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 84926c2d..0035b20c 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -117,7 +117,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { imu_.header.stamp = node_->now(); imu_.header.frame_id = frame_id_; - float deg_per_sec = ::pow(2, -12); + float rad_per_sec = ::pow(2, -12) * M_PI / 180.0F; float m_per_sec_sq = ::pow(2, -10); std::vector imu_data = m.data; @@ -140,9 +140,9 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { if (data_type == 14) { if (data_sign == 1) { - imu_.angular_velocity.x = 2048 - data_value * deg_per_sec; + imu_.angular_velocity.x = 2048 - data_value * rad_per_sec; } else { - imu_.angular_velocity.x = data_sign * data_value * deg_per_sec; + imu_.angular_velocity.x = data_sign * data_value * rad_per_sec; } } else if (data_type == 16) { //RCLCPP_INFO(node_->get_logger(), "data_sign: %f", data_sign); @@ -154,9 +154,9 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { } } else if (data_type == 13) { if (data_sign == 1) { - imu_.angular_velocity.y = 2048 - data_value * deg_per_sec; + imu_.angular_velocity.y = 2048 - data_value * rad_per_sec; } else { - imu_.angular_velocity.y = data_sign * data_value * deg_per_sec; + imu_.angular_velocity.y = data_sign * data_value * rad_per_sec; } } else if (data_type == 17) { if (data_sign == 1) { @@ -166,9 +166,9 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { } } else if (data_type == 5) { if (data_sign == 1) { - imu_.angular_velocity.z = 2048 - data_value * deg_per_sec; + imu_.angular_velocity.z = 2048 - data_value * rad_per_sec; } else { - imu_.angular_velocity.z = data_sign * data_value * deg_per_sec; + imu_.angular_velocity.z = data_sign * data_value * rad_per_sec; } } else if (data_type == 18) { if (data_sign == 1) { From 751c06455d3f9203287790e4333fbd84ffce2dc9 Mon Sep 17 00:00:00 2001 From: kagibson Date: Fri, 3 Feb 2023 06:08:16 -0800 Subject: [PATCH 137/152] Add serializer for NavTIMEGPS message. (#201) --- .../include/ublox_msgs/serialization.hpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 89046e74..0979377f 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -2366,6 +2366,36 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavTIMEGPS_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.valid); + stream.next(m.t_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavTIMEGPS_ & m) { + (void)m; + return 16; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavTIMEGPS_ & m) { + UbloxOStream stream(data, size); + stream.next(m.i_tow); + stream.next(m.f_tow); + stream.next(m.week); + stream.next(m.leap_s); + stream.next(m.valid); + stream.next(m.t_acc); + } +}; + /// /// @brief Serializes the RxmALM message which has a repeated block. /// From 9466188b40ea6f84a0224a2f4cf6351ca84037d5 Mon Sep 17 00:00:00 2001 From: jakor97 <51270271+jakor97@users.noreply.github.com> Date: Fri, 17 Mar 2023 15:41:30 +0100 Subject: [PATCH 138/152] Fix EsfStatus serialization (#208) Read missing `reserved1` bytes to fix incorrect values in `EsfStatus` message. Based on changes in master https://github.com/KumarRobotics/ublox/commit/5ce426951bdba87d3d581c021e6f0209bae08821 --- ublox_msgs/include/ublox_msgs/serialization.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 0979377f..bd5c7ab0 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1063,6 +1063,7 @@ struct UbloxSerializer > { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); stream.next(m.version); + stream.next(m.reserved1); stream.next(m.fusion_mode); stream.next(m.reserved2); stream.next(m.num_sens); @@ -1084,6 +1085,7 @@ struct UbloxSerializer > { UbloxOStream stream(data, size); stream.next(m.i_tow); stream.next(m.version); + stream.next(m.reserved1); stream.next(m.fusion_mode); stream.next(m.reserved2); stream.next(static_cast::_num_sens_type>(m.sens.size())); From be4fb7bc81daec759944b72d0d8bea665f33cecb Mon Sep 17 00:00:00 2001 From: Kieran Penner <128840781+khpenner@users.noreply.github.com> Date: Fri, 14 Apr 2023 12:13:45 -0500 Subject: [PATCH 139/152] Read & write all values from NavCOV message (#212) --- .../include/ublox_msgs/serialization.hpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index bd5c7ab0..29353bb3 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1512,9 +1512,18 @@ struct UbloxSerializer > { ublox_msgs::msg::NavCOV_ & m) { UbloxIStream stream(const_cast(data), count); stream.next(m.i_tow); + stream.next(m.version); stream.next(m.pos_cov_valid); stream.next(m.vel_cov_valid); - stream.next(m.reserved_0); + stream.next(m.reserved_0[0]); + stream.next(m.reserved_0[1]); + stream.next(m.reserved_0[2]); + stream.next(m.reserved_0[3]); + stream.next(m.reserved_0[4]); + stream.next(m.reserved_0[5]); + stream.next(m.reserved_0[6]); + stream.next(m.reserved_0[7]); + stream.next(m.reserved_0[8]); stream.next(m.pos_cov_nn); stream.next(m.pos_cov_ne); stream.next(m.pos_cov_nd); @@ -1538,9 +1547,18 @@ struct UbloxSerializer > { const ublox_msgs::msg::NavCOV_ & m) { UbloxOStream stream(data, size); stream.next(m.i_tow); + stream.next(m.version); stream.next(m.pos_cov_valid); stream.next(m.vel_cov_valid); - stream.next(m.reserved_0); + stream.next(m.reserved_0[0]); + stream.next(m.reserved_0[1]); + stream.next(m.reserved_0[2]); + stream.next(m.reserved_0[3]); + stream.next(m.reserved_0[4]); + stream.next(m.reserved_0[5]); + stream.next(m.reserved_0[6]); + stream.next(m.reserved_0[7]); + stream.next(m.reserved_0[8]); stream.next(m.pos_cov_nn); stream.next(m.pos_cov_ne); stream.next(m.pos_cov_nd); From 6fe76fd514d0e54987e6ab7bef1092a61abd7389 Mon Sep 17 00:00:00 2001 From: "Ryan D. Lewis" Date: Mon, 10 Jul 2023 14:14:27 -0400 Subject: [PATCH 140/152] Feature: Add support for new HPS firmware and publishing NMEA Sentences (#209) * Fix for NMEA messages overwhelming publishing queue * Apply suggestions from code review Co-authored-by: Chris Lalancette --- ublox_gps/CMakeLists.txt | 4 +- ublox_gps/include/ublox_gps/callback.hpp | 38 ++++++++++++++++++- ublox_gps/include/ublox_gps/gps.hpp | 7 ++++ ublox_gps/include/ublox_gps/node.hpp | 4 ++ ublox_gps/package.xml | 3 +- ublox_gps/src/gps.cpp | 4 ++ ublox_gps/src/node.cpp | 24 ++++++++++-- .../ublox_serialization/serialization.hpp | 16 +++++++- 8 files changed, 92 insertions(+), 8 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index a3a7b86a..feb23f02 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -17,15 +17,16 @@ find_package(asio REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nmea_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rtcm_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(ublox_msgs REQUIRED) find_package(ublox_serialization REQUIRED) -find_package(rtcm_msgs REQUIRED) # build node add_library(ublox_gps @@ -58,6 +59,7 @@ target_link_libraries(ublox_gps PUBLIC ${rcl_interfaces_TARGETS} rclcpp::rclcpp rclcpp_components::component + ${nmea_msgs_TARGETS} ${rtcm_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} diff --git a/ublox_gps/include/ublox_gps/callback.hpp b/ublox_gps/include/ublox_gps/callback.hpp index 01db8126..819a8ea9 100644 --- a/ublox_gps/include/ublox_gps/callback.hpp +++ b/ublox_gps/include/ublox_gps/callback.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -163,6 +164,15 @@ class CallbackHandlers final { std::make_shared>(callback, debug_))); } + /** + * @brief Add a callback handler for nmea messages + * @param callback the callback handler for the message + */ + void set_nmea_callback(std::function callback) { + std::lock_guard lock(callback_mutex_); + callback_nmea_ = callback; + } + /** * @brief Calls the callback handler for the message in the reader. * @param reader a reader containing a u-blox message @@ -178,6 +188,28 @@ class CallbackHandlers final { } } + /** + * @brief Calls the callback handler for the nmea messages in the reader. + * @param reader a reader containing an nmea message + */ + void handle_nmea(ublox::Reader& reader) { + std::lock_guard lock(callback_mutex_); + if (callback_nmea_ == nullptr) { + return; + } + + const std::string buffer = reader.getExtraData(); + size_t nmea_start = buffer.find('$', 0); + size_t nmea_end = buffer.find('\n', nmea_start); + while(nmea_start != std::string::npos && nmea_end != std::string::npos) { + std::string sentence = buffer.substr(nmea_start, nmea_end - nmea_start + 1); + callback_nmea_(sentence); + + nmea_start = buffer.find('$', nmea_end + 1); + nmea_end = buffer.find('\n', nmea_start); + } + } + /** * @brief Read a u-blox message of the given type. * @param message the received u-blox message @@ -225,12 +257,13 @@ class CallbackHandlers final { it != reader.pos() + reader.length() + 8; ++it) { oss << std::hex << static_cast(*it) << " "; } - // RCLCPP_DEBUG("U-blox: reading %d bytes\n%s", reader.length() + 8, + // RCLCPP_DEBUG(logger_, "U-blox: reading %d bytes\n%s", reader.length() + 8, // oss.str().c_str()); } handle(reader); } + handle_nmea(reader); // delete read bytes from ASIO input buffer std::copy(reader.pos(), reader.end(), data); @@ -246,6 +279,9 @@ class CallbackHandlers final { Callbacks callbacks_; std::mutex callback_mutex_; int debug_; + + //! Callback handler for nmea messages + std::function callback_nmea_{nullptr}; }; } // namespace ublox_gps diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 59070409..3f3f7a93 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -350,6 +351,12 @@ class Gps final { template void subscribe(typename CallbackHandler_::Callback callback); + /** + * @brief Subscribe to the given Ublox message. + * @param callback the callback handler for the message + */ + void subscribe_nmea(std::function callback); + /** * @brief Subscribe to the message with the given ID. This is used for * messages which have the same format but different message IDs, diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 20852893..e4d70575 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -42,6 +42,7 @@ #include #include #include +#include // Ublox GPS includes #include #include @@ -269,6 +270,9 @@ class UbloxNode final : public rclcpp::Node { rclcpp::Publisher::SharedPtr aid_alm_pub_; rclcpp::Publisher::SharedPtr aid_eph_pub_; rclcpp::Publisher::SharedPtr aid_hui_pub_; + rclcpp::Publisher::SharedPtr nmea_pub_; + + void publish_nmea(const std::string & sentence, const std::string & topic); //! Navigation rate in measurement cycles, see CfgRate.msg uint16_t nav_rate_{0}; diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index 4321fc93..d2134d4f 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -18,15 +18,16 @@ diagnostic_msgs diagnostic_updater geometry_msgs + nmea_msgs rcl_interfaces rclcpp rclcpp_components + rtcm_msgs sensor_msgs std_msgs tf2 ublox_msgs ublox_serialization - rtcm_msgs ament_cmake diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 806dc844..ad404d01 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -69,6 +69,10 @@ void Gps::setWorker(const std::shared_ptr& worker) { configured_ = static_cast(worker); } +void Gps::subscribe_nmea(std::function callback) { + callbacks_.set_nmea_callback(callback); +} + void Gps::subscribeAcks() { // Set NACK handler subscribeId(std::bind(&Gps::processNack, this, diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 2336b193..f1477bae 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include #include #include @@ -217,11 +219,11 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, const std::string & ref_rov) { - if (product_category == "HPG" && ref_rov == "REF") { + if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "REF") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); - } else if (product_category == "HPG" && ref_rov == "ROV") { + } else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") { components_.push_back(std::make_shared(nav_rate_, updater_, this)); - } else if (product_category == "HPG") { + } else if (product_category == "HPG" || product_category == "HPS") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); } else if (product_category == "TIM") { components_.push_back(std::make_shared(frame_id_, updater_, this)); @@ -441,6 +443,8 @@ void UbloxNode::getRosParams() { this->declare_parameter("publish.tim.tm2", false); + this->declare_parameter("publish.nmea", true); + // INF parameters this->declare_parameter("inf.all", true); this->declare_parameter("inf.debug", false); @@ -489,6 +493,10 @@ void UbloxNode::getRosParams() { if (getRosBoolean(this, "publish.aid.hui")) { aid_hui_pub_ = this->create_publisher("aidhui", 1); } + if (getRosBoolean(this, "publish.nmea")) { + // Larger queue depth to handle all NMEA strings being published consecutively + nmea_pub_ = this->create_publisher("nmea", 20); + } // Create subscriber for RTCM correction data to enable RTK this->subscription_ = this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); @@ -606,6 +614,16 @@ void UbloxNode::subscribe() { 1); } + if (getRosBoolean(this, "publish.nmea")) { + gps_->subscribe_nmea([this](const std::string &sentence) { + nmea_msgs::msg::Sentence m; + m.header.stamp = this->now(); + m.header.frame_id = frame_id_; + m.sentence = sentence; + nmea_pub_->publish(m); + }); + } + for (const std::shared_ptr & component : components_) { component->subscribe(gps_); } diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index b04fc81e..f86baddd 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -367,7 +368,10 @@ class Reader { */ Reader(const uint8_t *data, uint32_t count, const Options &options = Options()) : - data_(data), count_(count), found_(false), options_(options) {} + data_(data), count_(count), found_(false), options_(options) + { + extra_data_.reserve(1024); + } using iterator = const uint8_t *; @@ -386,6 +390,8 @@ class Reader { if (data_[0] == options_.sync_a && (count_ == 1 || data_[1] == options_.sync_b)) { break; + } else { + extra_data_.push_back(data_[0]); } } @@ -532,9 +538,15 @@ class Reader { return (classId() == class_id && messageId() == message_id); } - private: + const std::string &getExtraData() const { + return extra_data_; + } + +private: //! The buffer of message bytes const uint8_t *data_; + //! Unused data from the read buffer, contains nmea messages. + std::string extra_data_; //! the number of bytes in the buffer, //! decrement as the buffer is read uint32_t count_; //! Whether or not a message has been found From 8bc735e800b6bf8a56699946ae69408d34ba3dd4 Mon Sep 17 00:00:00 2001 From: Kieran Penner <128840781+khpenner@users.noreply.github.com> Date: Tue, 10 Oct 2023 08:15:27 -0500 Subject: [PATCH 141/152] Added CfgVALDEL, CfgVALGET & CfgVALSET messages (#218) * Added CfgVALDEL, CfgVALGET & CfgVALSET messages Co-authored-by: Brandon --- ublox_msgs/CMakeLists.txt | 4 + .../include/ublox_msgs/serialization.hpp | 114 ++++++++++++++++++ ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 7 ++ ublox_msgs/msg/CfgVALDEL.msg | 21 ++++ ublox_msgs/msg/CfgVALGET.msg | 23 ++++ ublox_msgs/msg/CfgVALSET.msg | 23 ++++ ublox_msgs/msg/CfgVALSETCfgdata.msg | 7 ++ ublox_msgs/src/ublox_msgs.cpp | 6 + 8 files changed, 205 insertions(+) create mode 100644 ublox_msgs/msg/CfgVALDEL.msg create mode 100644 ublox_msgs/msg/CfgVALGET.msg create mode 100644 ublox_msgs/msg/CfgVALSET.msg create mode 100644 ublox_msgs/msg/CfgVALSETCfgdata.msg diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 122cf216..4e3db305 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -42,6 +42,10 @@ set(msg_files "msg/CfgSBAS.msg" "msg/CfgTMODE3.msg" "msg/CfgUSB.msg" + "msg/CfgVALDEL.msg" + "msg/CfgVALGET.msg" + "msg/CfgVALSET.msg" + "msg/CfgVALSETCfgdata.msg" "msg/EsfINS.msg" "msg/EsfMEAS.msg" "msg/EsfRAWBlock.msg" diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 29353bb3..14418e39 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -894,6 +894,120 @@ struct UbloxSerializer > { } }; +/// +/// @brief Serializes the CfgVALDEL message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALDEL_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + deserialize(stream, m.keys[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALDEL_ &m) { + return 4 + 4 * m.keys.size(); + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALDEL_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + serialize(stream, m.keys[i]); + } + } +}; + +/// +/// @brief Serializes the CfgVALGET message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALGET_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.position); + uint8_t data_size = (count - 4) / 4; + m.keys.resize(data_size); + // the key and values are all together in the keys array + for (std::size_t i = 0; i < data_size; ++i) { + deserialize(stream, m.keys[i]); + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALGET_ &m) { + return 4 + 4 * m.keys.size(); + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALGET_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.position); + for (std::size_t i = 0; i < m.keys.size(); ++i) { + serialize(stream, m.keys[i]); + } + } +}; + +/// +/// @brief Serializes the CfgVALSET message which has a repeated block. +/// +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::CfgVALSET_ &m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + deserialize(stream, m.cfgdata[i].key); + for (std::size_t j = 0; j < m.cfgdata[i].data.size(); ++j) { + deserialize(stream, m.cfgdata[i].data[j]); + } + } + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::CfgVALSET_ &m) { + int data_size = 0; + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + data_size += m.cfgdata[i].data.size(); + data_size += 4; + } + return 4 + data_size; + } + + inline static void write(const uint8_t *data, uint32_t count, + const ublox_msgs::msg::CfgVALSET_ &m) { + UbloxOStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.layers); + stream.next(m.reserved0[0]); + stream.next(m.reserved0[1]); + for (std::size_t i = 0; i < m.cfgdata.size(); ++i) { + serialize(stream, m.cfgdata[i].key); + for (std::size_t j = 0; j < m.cfgdata[i].data.size(); ++j) { + serialize(stream, m.cfgdata[i].data[j]); + } + } + } +}; + template struct UbloxSerializer > { inline static void read(const uint8_t *data, uint32_t count, diff --git a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp index 2559a7b8..b1ac7155 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -85,6 +85,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -223,6 +227,9 @@ namespace Message { static const uint8_t SBAS = ublox_msgs::msg::CfgSBAS::MESSAGE_ID; static const uint8_t TMODE3 = ublox_msgs::msg::CfgTMODE3::MESSAGE_ID; static const uint8_t USB = ublox_msgs::msg::CfgUSB::MESSAGE_ID; + static const uint8_t VALDEL = ublox_msgs::msg::CfgVALDEL::MESSAGE_ID; + static const uint8_t VALGET = ublox_msgs::msg::CfgVALGET::MESSAGE_ID; + static const uint8_t VALSET = ublox_msgs::msg::CfgVALSET::MESSAGE_ID; } // namespace CFG namespace UPD { diff --git a/ublox_msgs/msg/CfgVALDEL.msg b/ublox_msgs/msg/CfgVALDEL.msg new file mode 100644 index 00000000..2c30d8b2 --- /dev/null +++ b/ublox_msgs/msg/CfgVALDEL.msg @@ -0,0 +1,21 @@ +# CFG-VALDEL (0x06 0x8c) +# Delete configuration item values +# +# This message is used to delete saved configurations to effectively revert the items values to +# defaults. Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 140 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # Bit mask: the layers where the configuration should be applied +uint8 LAYER_BBR = 1 # BBR layer +uint8 LAYER_FLASH = 2 # Flash Layer + +uint8[2] reserved0 + +# Start of repeated block +uint32[] keys # Configuration key IDs of configuration items to be retrieved +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALGET.msg b/ublox_msgs/msg/CfgVALGET.msg new file mode 100644 index 00000000..4780b807 --- /dev/null +++ b/ublox_msgs/msg/CfgVALGET.msg @@ -0,0 +1,23 @@ +# CFG-VALGET (0x06 0x8b) +# Get configuration items +# +# This message is used to get configuration values by providing a list of configuration key IDs, +# which identify the configuration items to retrieve. Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 139 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # The layer which the configuration items should be retrieved +uint8 LAYER_RAM = 0 # RAM layer +uint8 LAYER_BBR = 1 # BBR layer +uint8 LAYER_FLASH = 2 # Flash Layer +uint8 LAYER_DEFAULT = 7 # Default Layer + +uint16 position # Skip this many key values before constructing output message + +# Start of repeated block +uint32[] keys # Configuration key IDs of configuration items to be retrieved +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALSET.msg b/ublox_msgs/msg/CfgVALSET.msg new file mode 100644 index 00000000..daec4522 --- /dev/null +++ b/ublox_msgs/msg/CfgVALSET.msg @@ -0,0 +1,23 @@ +# CFG-VALSET (0x06 0x8a) +# Set configuration item values +# +# This message is used to set a configuration by providing configuration data ( a list of +# key and value pairs), which identify the configuration items to change, and their new values. +# Maximum of 64 Key IDs. +# + +uint8 CLASS_ID = 6 +uint8 MESSAGE_ID = 138 + +uint8 version # Message Version (0x00 for this version) + +uint8 layers # Bit mask: the layers where the configuration should be applied +uint8 LAYER_RAM = 1 # RAM layer +uint8 LAYER_BBR = 2 # BBR layer +uint8 LAYER_FLASH = 4 # Flash Layer + +uint8[2] reserved0 + +# Start of repeated block +CfgVALSETCfgdata[] cfgdata # Configuration data (key and value pairs) +# End of repeated block diff --git a/ublox_msgs/msg/CfgVALSETCfgdata.msg b/ublox_msgs/msg/CfgVALSETCfgdata.msg new file mode 100644 index 00000000..b0825367 --- /dev/null +++ b/ublox_msgs/msg/CfgVALSETCfgdata.msg @@ -0,0 +1,7 @@ +# See Cfg-VALSET +# + +uint32 key # Key for configuration item being changed + +uint8[] data # Data for configuration item being changed, + # array can be of length 1 - 8 (i.e. 1 - 8 bytes long) diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 3b9c7f73..ac905307 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -160,6 +160,12 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::TMODE3, ublox_msgs, CfgTMODE3) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::USB, ublox_msgs, CfgUSB) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALDEL, + ublox_msgs, CfgVALDEL); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALGET, + ublox_msgs, CfgVALGET); +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::CFG, ublox_msgs::Message::CFG::VALSET, + ublox_msgs, CfgVALSET); DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::UPD, ublox_msgs::Message::UPD::SOS, ublox_msgs, UpdSOS) From e668f03ff31c054a15a8838896bf08702fc2536a Mon Sep 17 00:00:00 2001 From: Jordan Slater <32441504+JordanSlater@users.noreply.github.com> Date: Tue, 10 Oct 2023 09:29:54 -0400 Subject: [PATCH 142/152] Updated Heading and Orientation Calculations to be Consistent with ROS 1 branch (#216) --- ublox_gps/src/hp_pos_rec_product.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ublox_gps/src/hp_pos_rec_product.cpp b/ublox_gps/src/hp_pos_rec_product.cpp index e89af884..51947d77 100644 --- a/ublox_gps/src/hp_pos_rec_product.cpp +++ b/ublox_gps/src/hp_pos_rec_product.cpp @@ -54,8 +54,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 imu_.angular_velocity_covariance[0] = -1; // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103). - // Alos convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id). - double heading = (static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI) - M_PI_2; + double heading = M_PI_2 - (static_cast(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI); tf2::Quaternion orientation; orientation.setRPY(0, 0, heading); imu_.orientation.x = orientation[0]; @@ -67,7 +66,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9 imu_.orientation_covariance[8] = 1000.0; // When heading is reported to be valid, use accuracy reported in 1e-5 deg units if (m.flags & ublox_msgs::msg::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID) { - imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2); + imu_.orientation_covariance[8] = ::pow(m.acc_heading * 1e-5 / 180.0 * M_PI, 2); } imu_pub_->publish(imu_); From 577ef65095bdc5f2cd1a2ea32a4dbf13dbcd0b7c Mon Sep 17 00:00:00 2001 From: brandonbeggs <52388984+brandonbeggs@users.noreply.github.com> Date: Sun, 5 Nov 2023 11:13:33 -0600 Subject: [PATCH 143/152] Added Firmware v9 GNSS configuration (#229) * Added Firmware v9 GNSS configuration * Update CfgVALSETCfgdata.msg Co-authored-by: Chris Lalancette --- .../include/ublox_gps/ublox_firmware8.hpp | 3 +- .../include/ublox_gps/ublox_firmware9.hpp | 24 +++- ublox_gps/src/node.cpp | 17 +++ ublox_gps/src/ublox_firmware9.cpp | 103 ++++++++++++++++++ ublox_msgs/msg/CfgVALSETCfgdata.msg | 25 +++++ 5 files changed, 169 insertions(+), 3 deletions(-) diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index 85223544..e5bbc895 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -61,7 +61,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { */ void subscribe(std::shared_ptr gps) override; - private: +protected: // Set from ROS parameters //! Whether or not to enable the Galileo GNSS bool enable_galileo_{false}; @@ -75,6 +75,7 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { bool clear_bbr_{false}; bool save_on_shutdown_{false}; +private: rclcpp::Publisher::SharedPtr nav_sat_pub_; rclcpp::Publisher::SharedPtr mon_hw_pub_; rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp index a9c08476..9e0a4d2b 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware9.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware9.hpp @@ -7,6 +7,9 @@ #include #include +#include +#include + #include #include #include @@ -15,12 +18,29 @@ namespace ublox_node { /** * @brief Implements functions for firmware version 9. - * For now it simply re-uses the firmware version 8 class - * but allows for future expansion of functionality */ class UbloxFirmware9 final : public UbloxFirmware8 { public: explicit UbloxFirmware9(const std::string & frame_id, std::shared_ptr updater, std::shared_ptr freq_diag, std::shared_ptr gnss, rclcpp::Node* node); + + /** + * @brief Configure settings specific to firmware 9 based on ROS parameters. + * + * @details Configure GNSS. The hardware has internal logic for + * detecting differences between the new and active GNSS + * configuration and will internally trigger a reset if necessary. + * Configure the NMEA if desired by the user. It also may clear the + * flash memory based on the ROS parameters. + */ + bool configureUblox(std::shared_ptr gps) override; + +private: + /** + * @brief Populate the CfgVALSETCfgData data type + * + * @details A helper function used to generate a configuration for a single signal. + */ + ublox_msgs::msg::CfgVALSETCfgdata generateSignalConfig(uint32_t signalID, bool enable); }; } // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index f1477bae..85cd615e 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -701,6 +701,23 @@ void UbloxNode::processMonVer() { } continue; } + // u-blox F9 modules support additional positioning signals + else if (strs[0] == "MOD") + { + std::vector moduleField; + moduleField = stringSplit(strs[1], "-"); + if (moduleField.size() > 1) + { + if (moduleField[1].substr(0,2) == "F9") + { + gnss_->add("GPS_L2C"); + gnss_->add("GAL_E5B"); + gnss_->add("BDS_B2"); + gnss_->add("QZSS_L2C"); + gnss_->add("GLO_L2"); + } + } + } } } // Last 1-2 lines contain supported GNSS diff --git a/ublox_gps/src/ublox_firmware9.cpp b/ublox_gps/src/ublox_firmware9.cpp index 320fe357..1eb3ddf4 100644 --- a/ublox_gps/src/ublox_firmware9.cpp +++ b/ublox_gps/src/ublox_firmware9.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -16,4 +17,106 @@ UbloxFirmware9::UbloxFirmware9(const std::string & frame_id, std::shared_ptr gps) +{ + if (clear_bbr_) + { + // clear flash memory + if (!gps->clearBbr()) + { + RCLCPP_ERROR(node_->get_logger(), "u-blox failed to clear flash memory"); + } + } + + gps->setSaveOnShutdown(save_on_shutdown_); + + // + // Configure the GNSS, only if the configuration is different + // + // First, get the current GNSS configuration + ublox_msgs::msg::CfgGNSS cfg_gnss; + if (gps->poll(cfg_gnss)) + { + RCLCPP_DEBUG(node_->get_logger(), "Read GNSS config."); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels in hardware: %i", cfg_gnss.num_trk_ch_hw); + RCLCPP_DEBUG(node_->get_logger(), "Num. tracking channels to use: %i", cfg_gnss.num_trk_ch_use); + } + else + { + throw std::runtime_error("Failed to read the GNSS config."); + } + + ublox_msgs::msg::CfgVALSET cfg_signal; + cfg_signal.layers = ublox_msgs::msg::CfgVALSET::LAYER_RAM; + + using signal = ublox_msgs::msg::CfgVALSETCfgdata; + + // Configure GPS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_ENABLE, enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L1CA_ENABLE, enable_gps_)); + if (gnss_->isSupported("GPS_L2C")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GPS_L2C_ENABLE, enable_gps_)); + } + + // Configure SBAS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_ENABLE, enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::SBAS_L1CA_ENABLE, enable_gps_)); + + // Configure Galileo Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_ENABLE, enable_galileo_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E1_ENABLE, enable_galileo_)); + if (gnss_->isSupported("GAL_E5B")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GAL_E5B_ENABLE, enable_galileo_)); + } + + // Configure Beidou Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_ENABLE, enable_beidou_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B1_ENABLE, enable_beidou_)); + if (gnss_->isSupported("BDS_B2")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::BDS_B2_ENABLE, enable_beidou_)); + } + + // Configure QZSS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_ENABLE, enable_qzss_ && enable_gps_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L1CA_ENABLE, enable_qzss_ && enable_gps_)); + if (gnss_->isSupported("QZSS_L2C")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::QZSS_L2C_ENABLE, enable_qzss_ && enable_gps_)); + } + + // Configure GLONASS Signals + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_ENABLE, enable_glonass_)); + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L1_ENABLE, enable_glonass_)); + if (gnss_->isSupported("GLO_L2")) + { + cfg_signal.cfgdata.push_back(generateSignalConfig(signal::GLO_L2_ENABLE, enable_glonass_)); + } + + RCLCPP_DEBUG(node_->get_logger(), "Ready to configure"); + gps->configure(cfg_signal, false); + + // + // NMEA config + // + if (getRosBoolean(node_, "nmea.set") && !gps->configure(cfg_nmea_)) + { + throw std::runtime_error("Failed to configure NMEA"); + } + + return true; +} + +ublox_msgs::msg::CfgVALSETCfgdata UbloxFirmware9::generateSignalConfig(uint32_t signalID, bool enable) +{ + ublox_msgs::msg::CfgVALSETCfgdata signalConfig; + signalConfig.key = signalID; + signalConfig.data.resize(1); + signalConfig.data[0] = enable; + return signalConfig; +} + + } // namespace ublox_node diff --git a/ublox_msgs/msg/CfgVALSETCfgdata.msg b/ublox_msgs/msg/CfgVALSETCfgdata.msg index b0825367..83d73dbe 100644 --- a/ublox_msgs/msg/CfgVALSETCfgdata.msg +++ b/ublox_msgs/msg/CfgVALSETCfgdata.msg @@ -1,3 +1,28 @@ +# CFG-SIGNAL Keys +uint32 GPS_ENABLE = 271646751 # 0x1031001f +uint32 GPS_L1CA_ENABLE = 271646721 # 0x10310001 +uint32 GPS_L2C_ENABLE = 271646723 # 0x10310003 + +uint32 SBAS_ENABLE = 271646752 # 0x10310020 +uint32 SBAS_L1CA_ENABLE = 271646725 # 0x10310005 + +uint32 GAL_ENABLE = 271646753 # 0x10310021 +uint32 GAL_E1_ENABLE = 271646727 # 0x10310007 +uint32 GAL_E5B_ENABLE = 271646730 # 0x1031000a + +uint32 BDS_ENABLE = 271646754 # 0x10310022 +uint32 BDS_B1_ENABLE = 271646733 # 0x1031000d +uint32 BDS_B2_ENABLE = 271646734 # 0x1031000e + +uint32 QZSS_ENABLE = 271646756 # 0x10310024 +uint32 QZSS_L1CA_ENABLE = 271646738 # 0x10310012 +uint32 QZSS_L1S_ENABLE = 271646740 # 0x10310014 +uint32 QZSS_L2C_ENABLE = 271646741 # 0x10310015 + +uint32 GLO_ENABLE = 271646757 # 0x10310025 +uint32 GLO_L1_ENABLE = 271646744 # 0x10310018 +uint32 GLO_L2_ENABLE = 271646746 # 0x1031001a + # See Cfg-VALSET # From f9396806c2e6febe7d55c1ba8adc9ab376eb4c44 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:09:23 +0300 Subject: [PATCH 144/152] fix ADR/UDR product detection --- ublox_gps/include/ublox_gps/adr_udr_product.hpp | 5 +++-- ublox_gps/src/adr_udr_product.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 4952a7c8..b00022df 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -28,7 +28,7 @@ namespace ublox_node { */ class AdrUdrProduct final : public virtual ComponentInterface { public: - explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); + explicit AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -64,6 +64,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning bool use_adr_; + float protocol_version_; sensor_msgs::msg::Imu imu_; sensor_msgs::msg::TimeReference t_ref_; @@ -89,4 +90,4 @@ class AdrUdrProduct final : public virtual ComponentInterface { } // namespace ublox_node -#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP +#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP \ No newline at end of file diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 0035b20c..7713ba7c 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -25,8 +25,8 @@ namespace ublox_node { // // u-blox ADR devices, partially implemented // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) - : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) +AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) + : protocol_version_(protocol_version) ,use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) { if (getRosBoolean(node_, "publish.esf.meas")) { imu_pub_ = @@ -63,7 +63,7 @@ void AdrUdrProduct::getRosParams() { } bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { - if (!gps->setUseAdr(use_adr_)) { + if (!gps->setUseAdr(use_adr_, protocol_version_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); } From 4542ddb1083b623af292c75053d91d9c71858671 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:09:59 +0300 Subject: [PATCH 145/152] fix: protocol version --- ublox_gps/include/ublox_gps/gps.hpp | 4 ++-- ublox_gps/src/gps.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 3f3f7a93..431d03df 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -300,7 +300,7 @@ class Gps final { * @note This is part of the expert settings. It is recommended you check * the ublox manual first. */ - bool setPpp(bool enable); + bool setPpp(bool enable, float protocol_version); /** * @brief Set the DGNSS mode (see CfgDGNSS message for details). @@ -314,7 +314,7 @@ class Gps final { * @param enable If true, enable ADR. * @return true on ACK, false on other conditions. */ - bool setUseAdr(bool enable); + bool setUseAdr(bool enable, float protocol_version); /** * @brief Configure the U-Blox to UTC time diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index ad404d01..87b1e078 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -555,11 +555,14 @@ bool Gps::setDeadReckonLimit(uint8_t limit) { return configure(msg); } -bool Gps::setPpp(bool enable) { +bool Gps::setPpp(bool enable, float protocol_version) { RCLCPP_DEBUG(logger_,"%s PPP", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_ppp = enable; + if(protocol_version >= 18){ + msg.version = 2; + } msg.mask1 = ublox_msgs::msg::CfgNAVX5::MASK1_PPP; return configure(msg); } @@ -571,11 +574,14 @@ bool Gps::setDgnss(uint8_t mode) { return configure(cfg); } -bool Gps::setUseAdr(bool enable) { +bool Gps::setUseAdr(bool enable, float protocol_version) { RCLCPP_DEBUG(logger_, "%s ADR/UDR", (enable ? "Enabling" : "Disabling")); ublox_msgs::msg::CfgNAVX5 msg; msg.use_adr = enable; + if(protocol_version >= 18){ + msg.version = 2; + } msg.mask2 = ublox_msgs::msg::CfgNAVX5::MASK2_ADR; return configure(msg); } From 95aa8185c6b3c6cc5869fdd257087dc65c47a679 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:10:20 +0300 Subject: [PATCH 146/152] fix: for protocol version >= 18 --- ublox_gps/src/node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..22e29721 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -229,11 +229,11 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(frame_id_, updater_, this)); } else if (product_category == "ADR" || product_category == "UDR") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); } else if (product_category == "FTS") { components_.push_back(std::make_shared()); } else if (product_category == "HPS") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); components_.push_back(std::make_shared(nav_rate_, updater_, this)); } else { RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", @@ -772,7 +772,7 @@ bool UbloxNode::configureUblox() { " SBAS."); } } - if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"))) { + if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"), protocol_version_)) { throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") + " PPP."); From 83f26d550a9ebbefc22e01c2056c5178454a3904 Mon Sep 17 00:00:00 2001 From: Muhammad540 Date: Mon, 11 Nov 2024 11:10:59 +0300 Subject: [PATCH 147/152] fix: neo_m8u_rover.yaml type issues --- ublox_gps/config/neo_m8u_rover.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ublox_gps/config/neo_m8u_rover.yaml b/ublox_gps/config/neo_m8u_rover.yaml index dc364feb..351b5d68 100644 --- a/ublox_gps/config/neo_m8u_rover.yaml +++ b/ublox_gps/config/neo_m8u_rover.yaml @@ -11,7 +11,7 @@ ublox_gps_node: device: /dev/ttyACM0 frame_id: m8u - rate: 4 # in Hz + rate: 4.0 # in Hz nav_rate: 4 # [# of measurement cycles], recommended 1 Hz, may # be either 5 Hz (Dual constellation) or # 8 Hz (GPS only) @@ -19,7 +19,7 @@ ublox_gps_node: # Max Alt: 50km # Max Horizontal Velocity: 250 m/s, # Max Vertical Velocity: 100 m/s - fix_mode: 3 + fix_mode: 3d enable_ppp: true dr_limit: 1 From 133a5d0f9e70c7b222bc09f6bf2a6113a169a006 Mon Sep 17 00:00:00 2001 From: Tomoaki Yokoyama Date: Fri, 19 Dec 2025 14:28:03 +0900 Subject: [PATCH 148/152] add driver reset service --- ublox_gps/CMakeLists.txt | 2 ++ ublox_gps/include/ublox_gps/node.hpp | 4 ++++ ublox_gps/package.xml | 1 + ublox_gps/src/node.cpp | 16 ++++++++++++++++ 4 files changed, 23 insertions(+) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index feb23f02..9472ea3a 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rclcpp_components REQUIRED) find_package(rtcm_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(ublox_msgs REQUIRED) find_package(ublox_serialization REQUIRED) @@ -63,6 +64,7 @@ target_link_libraries(ublox_gps PUBLIC ${rtcm_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} + ${std_srvs_TARGETS} tf2::tf2 ${ublox_msgs_TARGETS} ublox_serialization::ublox_serialization diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index e4d70575..5c92fcd3 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -37,6 +37,7 @@ // ROS includes #include #include +#include // U-Blox msgs nicludes #include #include @@ -299,6 +300,9 @@ class UbloxNode final : public rclcpp::Node { rclcpp::TimerBase::SharedPtr keep_alive_; rclcpp::TimerBase::SharedPtr poller_; + + rclcpp::Service::SharedPtr shutdown_srv_; + }; } // namespace ublox_node diff --git a/ublox_gps/package.xml b/ublox_gps/package.xml index d2134d4f..a09c76b9 100644 --- a/ublox_gps/package.xml +++ b/ublox_gps/package.xml @@ -25,6 +25,7 @@ rtcm_msgs sensor_msgs std_msgs + std_srvs tf2 ublox_msgs ublox_serialization diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 22e29721..850fd4b4 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -42,6 +42,7 @@ #include #include + #include #include #include @@ -500,6 +501,21 @@ void UbloxNode::getRosParams() { // Create subscriber for RTCM correction data to enable RTK this->subscription_ = this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); + + shutdown_srv_ = this->create_service( + "shutdown", + [this](const std::shared_ptr, + std::shared_ptr res) + { + res->success = true; + res->message = "Shutdown requested. Exiting..."; + + std::thread([](){ + // 少し待ってレスポンス返却を確実にする + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + rclcpp::shutdown(); + }).detach(); + }); } void UbloxNode::keepAlive() { From f5853357a0cd05611c7eec1a5b5445093c951725 Mon Sep 17 00:00:00 2001 From: Tomoaki Yokoyama Date: Mon, 22 Dec 2025 10:27:27 +0900 Subject: [PATCH 149/152] fix reset --- ublox_gps/include/ublox_gps/gps.hpp | 2 ++ ublox_gps/src/gps.cpp | 12 ++++++++++++ ublox_gps/src/node.cpp | 1 + 3 files changed, 15 insertions(+) diff --git a/ublox_gps/include/ublox_gps/gps.hpp b/ublox_gps/include/ublox_gps/gps.hpp index 431d03df..70f2b906 100644 --- a/ublox_gps/include/ublox_gps/gps.hpp +++ b/ublox_gps/include/ublox_gps/gps.hpp @@ -429,6 +429,8 @@ class Gps final { */ void setRawDataCallback(const Worker::WorkerRawCallback& callback); + bool hotrestart(); + private: //! Types for ACK/NACK messages, WAIT is used when waiting for an ACK enum AckType { diff --git a/ublox_gps/src/gps.cpp b/ublox_gps/src/gps.cpp index 87b1e078..d438d25d 100644 --- a/ublox_gps/src/gps.cpp +++ b/ublox_gps/src/gps.cpp @@ -300,6 +300,7 @@ void Gps::close() { RCLCPP_INFO(logger_, "U-Blox Flash BBR failed to save"); } } + hotrestart(); worker_.reset(); configured_ = false; } @@ -351,6 +352,7 @@ bool Gps::saveOnShutdown() { ublox_msgs::msg::CfgRST rst; rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_HOT_START; rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_GNSS_STOP; + if (!configure(rst)) { return false; } @@ -360,6 +362,16 @@ bool Gps::saveOnShutdown() { return configure(backup); } +bool Gps::hotrestart() { + ublox_msgs::msg::CfgRST rst; + // rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_COLD_START; + // rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_HW_IMMEDIATE; + rst.nav_bbr_mask = ublox_msgs::msg::CfgRST::NAV_BBR_HOT_START; + rst.reset_mode = ublox_msgs::msg::CfgRST::RESET_MODE_GNSS; + + return configure(rst); +} + bool Gps::clearBbr() { // Command saving the contents of BBR to flash memory // And wait for UBX-UPD-SOS-ACK diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 850fd4b4..a92775e8 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -944,6 +944,7 @@ void UbloxNode::initialize() { void UbloxNode::shutdown() { if (gps_->isInitialized()) { gps_->close(); + RCLCPP_INFO(this->get_logger(), "Closed connection to %s.", device_.c_str()); } } From 1896d08fef5e023f3623eb7d88d179129b2d7cd5 Mon Sep 17 00:00:00 2001 From: Tomoaki Yokoyama Date: Wed, 24 Dec 2025 10:56:26 +0900 Subject: [PATCH 150/152] remove delay --- ublox_gps/include/ublox_gps/node.hpp | 2 + ublox_gps/src/node.cpp | 356 +++++++++++++-------------- 2 files changed, 175 insertions(+), 183 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index 5c92fcd3..ec16c5ff 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -302,6 +302,8 @@ class UbloxNode final : public rclcpp::Node { rclcpp::TimerBase::SharedPtr poller_; rclcpp::Service::SharedPtr shutdown_srv_; + std::atomic_bool shutdown_requested_{false}; + rclcpp::TimerBase::SharedPtr shutdown_timer_; }; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index a92775e8..97df1606 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -27,22 +27,24 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. //============================================================================== -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include - +#include #include #include #include @@ -57,22 +59,17 @@ #include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace ublox_node { @@ -91,7 +88,7 @@ namespace ublox_node { * @return DynamicModel * @throws std::runtime_error on invalid argument. */ -uint8_t modelFromString(const std::string& model) { +uint8_t modelFromString(const std::string &model) { std::string lower = model; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "portable") { @@ -122,8 +119,7 @@ uint8_t modelFromString(const std::string& model) { return ublox_msgs::msg::CfgNAV5::DYN_MODEL_WRIST_WATCH; } - throw std::runtime_error("Invalid settings: " + lower + - " is not a valid dynamic model."); + throw std::runtime_error("Invalid settings: " + lower + " is not a valid dynamic model."); } /** @@ -135,7 +131,7 @@ uint8_t modelFromString(const std::string& model) { * @return FixMode * @throws std::runtime_error on invalid argument. */ -uint8_t fixModeFromString(const std::string& mode) { +uint8_t fixModeFromString(const std::string &mode) { std::string lower = mode; std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); if (lower == "2d") { @@ -148,12 +144,10 @@ uint8_t fixModeFromString(const std::string& mode) { return ublox_msgs::msg::CfgNAV5::FIX_MODE_AUTO; } - throw std::runtime_error("Invalid settings: " + mode + - " is not a valid fix mode."); + throw std::runtime_error("Invalid settings: " + mode + " is not a valid fix mode."); } -std::vector stringSplit(const std::string &str, - const std::string &splitter) { +std::vector stringSplit(const std::string &str, const std::string &splitter) { std::vector ret; size_t next = 0; size_t current = next; @@ -176,7 +170,7 @@ std::vector stringSplit(const std::string &str, // // u-blox ROS Node // -UbloxNode::UbloxNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ublox_gps_node", options) { +UbloxNode::UbloxNode(const rclcpp::NodeOptions &options) : rclcpp::Node("ublox_gps_node", options) { int debug = this->declare_parameter("debug", 1); if (debug) { if (rcutils_logging_set_logger_level("ublox_gps_node", RCUTILS_LOG_SEVERITY_DEBUG) != RCUTILS_RET_OK) { @@ -203,13 +197,16 @@ void UbloxNode::addFirmwareInterface() { if (protocol_version_ < 14.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 6; - } else if (protocol_version_ >= 14.0 && protocol_version_ <= 15.0) { + } + else if (protocol_version_ >= 14.0 && protocol_version_ <= 15.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 7; - } else if (protocol_version_ > 15.0 && protocol_version_ <= 23.0) { + } + else if (protocol_version_ > 15.0 && protocol_version_ <= 23.0) { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 8; - } else { + } + else { components_.push_back(std::make_shared(frame_id_, updater_, freq_diag_, gnss_, this)); ublox_version = 9; } @@ -217,29 +214,33 @@ void UbloxNode::addFirmwareInterface() { RCLCPP_INFO(this->get_logger(), "U-Blox Firmware Version: %d", ublox_version); } - -void UbloxNode::addProductInterface(const std::string & product_category, - const std::string & ref_rov) { +void UbloxNode::addProductInterface(const std::string &product_category, const std::string &ref_rov) { if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "REF") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); - } else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") { + } + else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") { components_.push_back(std::make_shared(nav_rate_, updater_, this)); - } else if (product_category == "HPG" || product_category == "HPS") { + } + else if (product_category == "HPG" || product_category == "HPS") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); - } else if (product_category == "TIM") { + } + else if (product_category == "TIM") { components_.push_back(std::make_shared(frame_id_, updater_, this)); - } else if (product_category == "ADR" || - product_category == "UDR") { + } + else if (product_category == "ADR" || product_category == "UDR") { components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); - } else if (product_category == "FTS") { + } + else if (product_category == "FTS") { components_.push_back(std::make_shared()); - } else if (product_category == "HPS") { + } + else if (product_category == "HPS") { components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); components_.push_back(std::make_shared(nav_rate_, updater_, this)); - } else { - RCLCPP_WARN(this->get_logger(), "Product category %s %s from MonVER message not recognized %s", - product_category.c_str(), ref_rov.c_str(), - "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, HPS"); + } + else { + RCLCPP_WARN( + this->get_logger(), "Product category %s %s from MonVER message not recognized %s", product_category.c_str(), ref_rov.c_str(), + "options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, HPS"); } } @@ -255,9 +256,8 @@ void UbloxNode::getRosParams() { // UART 1 params baudrate_ = declareRosIntParameter(this, "uart1.baudrate", 9600); - uart_in_ = declareRosIntParameter(this, "uart1.in", ublox_msgs::msg::CfgPRT::PROTO_UBX - | ublox_msgs::msg::CfgPRT::PROTO_NMEA - | ublox_msgs::msg::CfgPRT::PROTO_RTCM); + uart_in_ = declareRosIntParameter( + this, "uart1.in", ublox_msgs::msg::CfgPRT::PROTO_UBX | ublox_msgs::msg::CfgPRT::PROTO_NMEA | ublox_msgs::msg::CfgPRT::PROTO_RTCM); uart_out_ = declareRosIntParameter(this, "uart1.out", ublox_msgs::msg::CfgPRT::PROTO_UBX); // USB params set_usb_ = false; @@ -267,12 +267,10 @@ void UbloxNode::getRosParams() { if (isRosParameterSet(this, "usb.in") || isRosParameterSet(this, "usb.out")) { set_usb_ = true; if (!getRosUint(this, "usb.in", usb_in_)) { - throw std::runtime_error(std::string("usb.out is set, therefore ") + - "usb.in must be set"); + throw std::runtime_error(std::string("usb.out is set, therefore ") + "usb.in must be set"); } if (!getRosUint(this, "usb.out", usb_out_)) { - throw std::runtime_error(std::string("usb.in is set, therefore ") + - "usb.out must be set"); + throw std::runtime_error(std::string("usb.in is set, therefore ") + "usb.out must be set"); } } // Measurement rate params @@ -290,8 +288,7 @@ void UbloxNode::getRosParams() { this->get_parameter("rtcm.rates", rtcm_rates); if (rtcm_ids.size() != rtcm_rates.size()) { - throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + - " must match size of rtcm_rates"); + throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") + " must match size of rtcm_rates"); } rtcms_.resize(rtcm_ids.size()); @@ -320,14 +317,13 @@ void UbloxNode::getRosParams() { this->declare_parameter("gnss.galileo", false); this->declare_parameter("gnss.beidou", false); this->declare_parameter("gnss.imes", false); - max_sbas_ = declareRosIntParameter(this, "sbas.max", 0); // Maximum number of SBAS channels + max_sbas_ = declareRosIntParameter(this, "sbas.max", 0); // Maximum number of SBAS channels sbas_usage_ = declareRosIntParameter(this, "sbas.usage", 0); dynamic_model_ = this->declare_parameter("dynamic_model", std::string("portable")); dmodel_ = modelFromString(dynamic_model_); fix_mode_ = this->declare_parameter("fix_mode", std::string("auto")); fmode_ = fixModeFromString(fix_mode_); - dr_limit_ = declareRosIntParameter(this, "dr_limit", 0); // Dead reckoning limit - + dr_limit_ = declareRosIntParameter(this, "dr_limit", 0); // Dead reckoning limit this->declare_parameter("dat.set", false); this->declare_parameter("dat.majA", rclcpp::PARAMETER_DOUBLE); @@ -337,17 +333,12 @@ void UbloxNode::getRosParams() { this->declare_parameter("dat.scale", rclcpp::PARAMETER_DOUBLE); if (getRosBoolean(this, "dat.set")) { std::vector shift, rot; - if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) - || !this->get_parameter("dat.flat", cfg_dat_.flat) - || !this->get_parameter("dat.shift", shift) - || !this->get_parameter("dat.rot", rot) - || !this->get_parameter("dat.scale", cfg_dat_.scale)) { - throw std::runtime_error(std::string("dat.set is true, therefore ") + - "dat.majA, dat.flat, dat.shift, dat.rot, & dat.scale must be set"); + if (!this->get_parameter("dat.majA", cfg_dat_.maj_a) || !this->get_parameter("dat.flat", cfg_dat_.flat) || + !this->get_parameter("dat.shift", shift) || !this->get_parameter("dat.rot", rot) || !this->get_parameter("dat.scale", cfg_dat_.scale)) { + throw std::runtime_error(std::string("dat.set is true, therefore ") + "dat.majA, dat.flat, dat.shift, dat.rot, & dat.scale must be set"); } if (shift.size() != 3 || rot.size() != 3) { - throw std::runtime_error(std::string("size of dat.shift & dat.rot ") + - "must be 3"); + throw std::runtime_error(std::string("size of dat.shift & dat.rot ") + "must be 3"); } checkRange(cfg_dat_.maj_a, 6300000.0, 6500000.0, "dat.majA"); checkRange(cfg_dat_.flat, 0.0, 500.0, "dat.flat"); @@ -384,8 +375,7 @@ void UbloxNode::getRosParams() { // raw data stream logging this->declare_parameter("raw_data_stream.enable", false); if (getRosBoolean(this, "raw_data_stream.enable")) { - raw_data_stream_pa_ = std::make_shared( - getRosBoolean(this, "raw_data_stream.enable")); + raw_data_stream_pa_ = std::make_shared(getRosBoolean(this, "raw_data_stream.enable")); raw_data_stream_pa_->getRosParams(); } @@ -500,21 +490,23 @@ void UbloxNode::getRosParams() { } // Create subscriber for RTCM correction data to enable RTK - this->subscription_ = this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); + this->subscription_ = + this->create_subscription("/rtcm", 10, std::bind(&UbloxNode::rtcmCallback, this, std::placeholders::_1)); + + shutdown_timer_ = this->create_wall_timer(std::chrono::milliseconds(0), [this]() { + if (!shutdown_requested_.exchange(false)) + return; + rclcpp::shutdown(); + }); + shutdown_timer_->cancel(); // 普段は止めておく shutdown_srv_ = this->create_service( - "shutdown", - [this](const std::shared_ptr, - std::shared_ptr res) - { + "shutdown", [this](const std::shared_ptr, + std::shared_ptr res) { res->success = true; res->message = "Shutdown requested. Exiting..."; - - std::thread([](){ - // 少し待ってレスポンス返却を確実にする - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - rclcpp::shutdown(); - }).detach(); + shutdown_requested_ = true; + shutdown_timer_->reset(); // タイマースタート }); } @@ -544,11 +536,14 @@ void UbloxNode::pollMessages() { void UbloxNode::printInf(const ublox_msgs::msg::Inf &m, uint8_t id) { if (id == ublox_msgs::Message::INF::ERROR) { RCLCPP_ERROR(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); - } else if (id == ublox_msgs::Message::INF::WARNING) { + } + else if (id == ublox_msgs::Message::INF::WARNING) { RCLCPP_WARN(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); - } else if (id == ublox_msgs::Message::INF::DEBUG) { + } + else if (id == ublox_msgs::Message::INF::DEBUG) { RCLCPP_DEBUG(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); - } else { + } + else { RCLCPP_INFO(this->get_logger(), "INF: %s", std::string(m.str.begin(), m.str.end()).c_str()); } } @@ -559,75 +554,86 @@ void UbloxNode::subscribe() { // Nav Messages if (getRosBoolean(this, "publish.nav.status")) { - gps_->subscribe([this](const ublox_msgs::msg::NavSTATUS &m) { nav_status_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::NavSTATUS &m) { + nav_status_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.nav.posecef")) { - gps_->subscribe([this](const ublox_msgs::msg::NavPOSECEF &m) { nav_posecef_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::NavPOSECEF &m) { + nav_posecef_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.nav.clock")) { - gps_->subscribe([this](const ublox_msgs::msg::NavCLOCK &m) { nav_clock_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::NavCLOCK &m) { + nav_clock_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.nav.cov")) { - gps_->subscribe([this](const ublox_msgs::msg::NavCOV &m) { nav_cov_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::NavCOV &m) { + nav_cov_pub_->publish(m); + }, + 1); } // INF messages if (getRosBoolean(this, "inf.debug")) { gps_->subscribeId( - std::bind(&UbloxNode::printInf, this, std::placeholders::_1, - ublox_msgs::Message::INF::DEBUG), - ublox_msgs::Message::INF::DEBUG); + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::DEBUG), ublox_msgs::Message::INF::DEBUG); } if (getRosBoolean(this, "inf.error")) { gps_->subscribeId( - std::bind(&UbloxNode::printInf, this, std::placeholders::_1, - ublox_msgs::Message::INF::ERROR), - ublox_msgs::Message::INF::ERROR); + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::ERROR), ublox_msgs::Message::INF::ERROR); } if (getRosBoolean(this, "inf.notice")) { gps_->subscribeId( - std::bind(&UbloxNode::printInf, this, std::placeholders::_1, - ublox_msgs::Message::INF::NOTICE), - ublox_msgs::Message::INF::NOTICE); + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::NOTICE), ublox_msgs::Message::INF::NOTICE); } if (getRosBoolean(this, "inf.test")) { gps_->subscribeId( - std::bind(&UbloxNode::printInf, this, std::placeholders::_1, - ublox_msgs::Message::INF::TEST), - ublox_msgs::Message::INF::TEST); + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::TEST), ublox_msgs::Message::INF::TEST); } if (getRosBoolean(this, "inf.warning")) { gps_->subscribeId( - std::bind(&UbloxNode::printInf, this, std::placeholders::_1, - ublox_msgs::Message::INF::WARNING), - ublox_msgs::Message::INF::WARNING); + std::bind(&UbloxNode::printInf, this, std::placeholders::_1, ublox_msgs::Message::INF::WARNING), ublox_msgs::Message::INF::WARNING); } // AID messages if (getRosBoolean(this, "publish.aid.alm")) { - gps_->subscribe([this](const ublox_msgs::msg::AidALM &m) { aid_alm_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::AidALM &m) { + aid_alm_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.aid.eph")) { - gps_->subscribe([this](const ublox_msgs::msg::AidEPH &m) { aid_eph_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::AidEPH &m) { + aid_eph_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.aid.hui")) { - gps_->subscribe([this](const ublox_msgs::msg::AidHUI &m) { aid_hui_pub_->publish(m); }, - 1); + gps_->subscribe( + [this](const ublox_msgs::msg::AidHUI &m) { + aid_hui_pub_->publish(m); + }, + 1); } if (getRosBoolean(this, "publish.nmea")) { @@ -640,13 +646,13 @@ void UbloxNode::subscribe() { }); } - for (const std::shared_ptr & component : components_) { + for (const std::shared_ptr &component : components_) { component->subscribe(gps_); } } void UbloxNode::initializeRosDiagnostics() { - for (const std::shared_ptr & component : components_) { + for (const std::shared_ptr &component : components_) { component->initializeRosDiagnostics(); } } @@ -657,27 +663,25 @@ void UbloxNode::processMonVer() { throw std::runtime_error("Failed to poll MonVER & set relevant settings"); } - RCLCPP_INFO(this->get_logger(), "%s, HW VER: %s", - std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(), - std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str()); + RCLCPP_INFO( + this->get_logger(), "%s, HW VER: %s", std::string(monVer.sw_version.begin(), monVer.sw_version.end()).c_str(), + std::string(monVer.hw_version.begin(), monVer.hw_version.end()).c_str()); // Convert extension to vector of strings std::vector extensions; extensions.reserve(monVer.extension.size()); for (std::size_t i = 0; i < monVer.extension.size(); ++i) { // NOLINT(modernize-loop-convert) - RCLCPP_DEBUG(this->get_logger(), "%s", - std::string(monVer.extension[i].field.begin(), monVer.extension[i].field.end()).c_str()); + RCLCPP_DEBUG(this->get_logger(), "%s", std::string(monVer.extension[i].field.begin(), monVer.extension[i].field.end()).c_str()); // Find the end of the string (null character) - unsigned char* end = std::find(monVer.extension[i].field.begin(), - monVer.extension[i].field.end(), '\0'); + unsigned char *end = std::find(monVer.extension[i].field.begin(), monVer.extension[i].field.end(), '\0'); extensions.emplace_back(std::string(monVer.extension[i].field.begin(), end)); } // Get the protocol version - for (const std::string & ext : extensions) { + for (const std::string &ext : extensions) { std::size_t found = ext.find("PROTVER"); if (found != std::string::npos) { - const char * sub = ext.substr(8, ext.size()-8).c_str(); - char * end{nullptr}; + const char *sub = ext.substr(8, ext.size() - 8).c_str(); + char *end{nullptr}; protocol_version_ = std::strtof(sub, &end); if (protocol_version_ == HUGE_VALF || (protocol_version_ == 0 && end == sub)) { // strtof failed to convert either via overflow or no conversion possible. @@ -688,8 +692,7 @@ void UbloxNode::processMonVer() { } } if (protocol_version_ == 0.0) { - RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", - "Defaulting to firmware version 6."); + RCLCPP_WARN(this->get_logger(), "Failed to parse MonVER and determine protocol version. %s", "Defaulting to firmware version 6."); } addFirmwareInterface(); @@ -699,10 +702,11 @@ void UbloxNode::processMonVer() { if (extensions.size() > 0) { strs = stringSplit(extensions[extensions.size() - 1], ";"); } - for (const std::string & str : strs) { + for (const std::string &str : strs) { gnss_->add(str); } - } else { + } + else { for (std::size_t i = 0; i < extensions.size(); ++i) { std::vector strs; // Up to 2nd to last line @@ -712,20 +716,18 @@ void UbloxNode::processMonVer() { if (strs[0] == "FWVER") { if (strs[1].length() > 8) { addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); - } else { + } + else { addProductInterface(strs[1].substr(0, 3)); } continue; } // u-blox F9 modules support additional positioning signals - else if (strs[0] == "MOD") - { + else if (strs[0] == "MOD") { std::vector moduleField; moduleField = stringSplit(strs[1], "-"); - if (moduleField.size() > 1) - { - if (moduleField[1].substr(0,2) == "F9") - { + if (moduleField.size() > 1) { + if (moduleField[1].substr(0, 2) == "F9") { gnss_->add("GPS_L2C"); gnss_->add("GAL_E5B"); gnss_->add("BDS_B2"); @@ -739,7 +741,7 @@ void UbloxNode::processMonVer() { // Last 1-2 lines contain supported GNSS if (i >= extensions.size() - 2) { strs = stringSplit(extensions[i], ";"); - for (const std::string & str : strs) { + for (const std::string &str : strs) { gnss_->add(str); } } @@ -755,17 +757,14 @@ bool UbloxNode::configureUblox() { if (load_.load_mask != 0) { RCLCPP_DEBUG(this->get_logger(), "Loading u-blox configuration from memory. %u", load_.load_mask); if (!gps_->configure(load_)) { - throw std::runtime_error(std::string("Failed to load configuration ") + - "from memory"); + throw std::runtime_error(std::string("Failed to load configuration ") + "from memory"); } if (load_.load_mask & ublox_msgs::msg::CfgCFG::MASK_IO_PORT) { - RCLCPP_DEBUG(this->get_logger(), "Loaded I/O configuration from memory, resetting serial %s", - "communications."); + RCLCPP_DEBUG(this->get_logger(), "Loaded I/O configuration from memory, resetting serial %s", "communications."); std::chrono::seconds wait(kResetWait); gps_->reset(wait); if (!gps_->isConfigured()) { - throw std::runtime_error(std::string("Failed to reset serial I/O") + - "after loading I/O configurations from device memory."); + throw std::runtime_error(std::string("Failed to reset serial I/O") + "after loading I/O configurations from device memory."); } } } @@ -776,22 +775,17 @@ bool UbloxNode::configureUblox() { } if (!gps_->configRate(meas_rate_, nav_rate_)) { std::stringstream ss; - ss << "Failed to set measurement rate to " << meas_rate_ - << "ms and navigation rate to " << nav_rate_; + ss << "Failed to set measurement rate to " << meas_rate_ << "ms and navigation rate to " << nav_rate_; throw std::runtime_error(ss.str()); } // If device doesn't have SBAS, will receive NACK (causes exception) if (gnss_->isSupported("SBAS")) { if (!gps_->configSbas(getRosBoolean(this, "gnss.sbas"), sbas_usage_, max_sbas_)) { - throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(this, "gnss.sbas") ? "enable" : "disable") + - " SBAS."); + throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "gnss.sbas") ? "enable" : "disable") + " SBAS."); } } if (!gps_->setPpp(getRosBoolean(this, "enable_ppp"), protocol_version_)) { - throw std::runtime_error(std::string("Failed to ") + - (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") - + " PPP."); + throw std::runtime_error(std::string("Failed to ") + (getRosBoolean(this, "enable_ppp") ? "enable" : "disable") + " PPP."); } if (!gps_->setDynamicModel(dmodel_)) { throw std::runtime_error("Failed to set model: " + dynamic_model_ + "."); @@ -808,20 +802,20 @@ bool UbloxNode::configureUblox() { throw std::runtime_error("Failed to set user-defined datum."); } // Configure each component - for (const std::shared_ptr & component : components_) { + for (const std::shared_ptr &component : components_) { if (!component->configureUblox(gps_)) { return false; } } } if (save_.save_mask != 0) { - RCLCPP_DEBUG(this->get_logger(), "Saving the u-blox configuration, mask %u, device %u", - save_.save_mask, save_.device_mask); + RCLCPP_DEBUG(this->get_logger(), "Saving the u-blox configuration, mask %u, device %u", save_.save_mask, save_.device_mask); if (!gps_->configure(save_)) { RCLCPP_ERROR(this->get_logger(), "u-blox unable to save configuration to non-volatile memory"); } } - } catch (const std::exception& e) { + } + catch (const std::exception &e) { RCLCPP_FATAL(this->get_logger(), "Error configuring u-blox: %s", e.what()); return false; } @@ -866,34 +860,32 @@ void UbloxNode::initializeIo() { gps_->setConfigOnStartup(getRosBoolean(this, "config_on_startup")); std::smatch match; - if (std::regex_match(device_, match, - std::regex("(tcp|udp)://(.+):(\\d+)"))) { + if (std::regex_match(device_, match, std::regex("(tcp|udp)://(.+):(\\d+)"))) { std::string proto(match[1]); if (proto == "tcp") { std::string host(match[2]); std::string port(match[3]); - RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps_->initializeTcp(host, port); - } else if (proto == "udp") { + } + else if (proto == "udp") { std::string host(match[2]); std::string port(match[3]); - RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), - port.c_str()); + RCLCPP_INFO(this->get_logger(), "Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str()); gps_->initializeUdp(host, port); - } else { + } + else { throw std::runtime_error("Protocol '" + proto + "' is unsupported"); } - } else { + } + else { gps_->initializeSerial(device_, baudrate_, uart_in_, uart_out_); } // raw data stream logging if (getRosBoolean(this, "raw_data_stream.enable")) { if (raw_data_stream_pa_->isEnabled()) { - gps_->setRawDataCallback( - std::bind(&RawDataStreamPa::ubloxCallback, raw_data_stream_pa_.get(), - std::placeholders::_1, std::placeholders::_2)); + gps_->setRawDataCallback(std::bind(&RawDataStreamPa::ubloxCallback, raw_data_stream_pa_.get(), std::placeholders::_1, std::placeholders::_2)); raw_data_stream_pa_->initialize(); } } @@ -904,9 +896,7 @@ void UbloxNode::initialize() { getRosParams(); // configure diagnostic updater for frequency - freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, - kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); - + freq_diag_ = std::make_shared(std::string("fix"), kFixFreqTol, kFixFreqWindow, kTimeStampStatusMin, nav_rate_, meas_rate_, updater_); initializeIo(); // Must process Mon VER before setting firmware/hardware params @@ -917,7 +907,7 @@ void UbloxNode::initialize() { } } // Must set firmware & hardware params before initializing diagnostics - for (const std::shared_ptr & component : components_) { + for (const std::shared_ptr &component : components_) { component->getRosParams(); } // Do this last @@ -932,12 +922,12 @@ void UbloxNode::initialize() { if (device_.substr(0, 6) == "udp://") { // Setup timer to poll version message to keep UDP socket active - keep_alive_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(kKeepAlivePeriod * 1000.0)), - std::bind(&UbloxNode::keepAlive, this)); + keep_alive_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(kKeepAlivePeriod * 1000.0)), std::bind(&UbloxNode::keepAlive, this)); } - poller_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(kPollDuration * 1000.0)), - std::bind(&UbloxNode::pollMessages, this)); + poller_ = + this->create_wall_timer(std::chrono::milliseconds(static_cast(kPollDuration * 1000.0)), std::bind(&UbloxNode::pollMessages, this)); } } From 9222b993505f57b6b4aeef044d665bfb0b3860bb Mon Sep 17 00:00:00 2001 From: nomumu Date: Wed, 1 Jul 2026 17:13:54 +0900 Subject: [PATCH 151/152] Add try/catch code to handle exceptions --- ublox_gps/src/node_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ublox_gps/src/node_main.cpp b/ublox_gps/src/node_main.cpp index aeb38bfe..dcdff1ff 100644 --- a/ublox_gps/src/node_main.cpp +++ b/ublox_gps/src/node_main.cpp @@ -10,9 +10,15 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + int exit_code = 0; + try { + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("ublox_gps_node"), "Fatal error: %s", e.what()); + exit_code = 1; + } rclcpp::shutdown(); - return 0; + return exit_code; } From aa3f4ad1e612f54cf35843d369074090bbd27c70 Mon Sep 17 00:00:00 2001 From: dev Date: Tue, 14 Jul 2026 17:18:23 +0900 Subject: [PATCH 152/152] fix time threshold --- ublox_gps/include/ublox_gps/node.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index ec16c5ff..4e54352c 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -91,11 +91,11 @@ class UbloxNode final : public rclcpp::Node { constexpr static double kPollDuration = 1.0; // Constants used for diagnostic frequency updater //! [s] 5Hz diagnostic period - const float kDiagnosticPeriod = 0.2; + const float kDiagnosticPeriod = 0.5; //! Tolerance for Fix topic frequency as percentage of target frequency - const double kFixFreqTol = 0.15; + const double kFixFreqTol = 0.3; //! Window [num messages] for Fix Frequency Diagnostic - const double kFixFreqWindow = 10; + const double kFixFreqWindow = 25; //! Minimum Time Stamp Status for fix frequency diagnostic const double kTimeStampStatusMin = 0;