From 07c2644fa200725e81596e330386e4d2330ed25f Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 14:18:21 -0800 Subject: [PATCH 01/13] Rebase on more recent remote --- README.md | 4 + ublox_gps/CMakeLists.txt | 1 + ublox_gps/config/zed_f9r.yaml | 26 ++ .../include/ublox_gps/adr_udr_product.hpp | 3 + .../include/ublox_gps/hpg_dr_product.hpp | 123 ++++++ ublox_gps/include/ublox_gps/node.hpp | 5 +- .../include/ublox_gps/ublox_firmware8.hpp | 4 + ublox_gps/include/ublox_gps/utils.hpp | 1 + ublox_gps/src/adr_udr_product.cpp | 193 +++++--- ublox_gps/src/hpg_dr_product.cpp | 417 ++++++++++++++++++ ublox_gps/src/node.cpp | 48 +- ublox_mcu-config | 8 + ublox_msgs/CMakeLists.txt | 1 + .../include/ublox_msgs/serialization.hpp | 46 ++ ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 2 + ublox_msgs/msg/EsfMEAS.msg | 2 +- ublox_msgs/msg/NavHPPOSLLH.msg | 32 ++ ublox_msgs/src/ublox_msgs.cpp | 4 +- .../ublox_serialization/serialization.hpp | 1 + 19 files changed, 836 insertions(+), 85 deletions(-) create mode 100644 ublox_gps/config/zed_f9r.yaml create mode 100644 ublox_gps/include/ublox_gps/hpg_dr_product.hpp create mode 100644 ublox_gps/src/hpg_dr_product.cpp create mode 100644 ublox_mcu-config create mode 100644 ublox_msgs/msg/NavHPPOSLLH.msg diff --git a/README.md b/README.md index 15abbe93..e5da26ee 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ Example .yaml configuration files are included in `ublox_gps/config`. Consult th The `ublox_gps` node supports the following parameters for all products and firmware versions: * `device`: Path to the device port. Defaults to `/dev/ttyACM0`. * `raw_data`: Whether the device is a raw data product. Defaults to false. Firmware <= 7.03 only. +* `config_on_startup`: Whether the node should configure the device (true) or use the device's configuration (false). * `load`: Parameters for loading the configuration to non-volatile memory. See `ublox_msgs/CfgCFG.msg` * `load/mask`: uint32_t. Mask of the configurations to load. * `load/device`: uint32_t. Mask which selects the devices for the load command. @@ -170,6 +171,7 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below * `publish/nav/all`: This is the default value for the `publish/mon/` parameters below. It defaults to `publish/all`. Individual messages can be enabled or disabled by setting the parameters below. * `publish/nav/att`: Topic `~navatt`. **ADR/UDR devices only** * `publish/nav/clock`: Topic `~navclock` +* `publish/nav/hpposllh`: Topic `~hpposllh` **Firmware >= 8 High-Precision GNSS Devices only**. * `publish/nav/posecef`: Topic `~navposecef` * `publish/nav/posllh`: Topic `~navposllh`. **Firmware <= 6 only.** For firmware 7 and above, see NavPVT * `publish/nav/pvt`: Topic `~navpvt`. **Firmware >= 7 only.** @@ -188,6 +190,8 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below * `publish/esf/meas`: Topic `~esfmeas` * `publish/esf/raw`: Topic `~esfraw` * `publish/esf/status`: Topic `~esfstatus` +* Topic `~imu`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) +* Topic `~imu_raw`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) ### HNR messages * `publish/hnr/pvt`: Topic `~hnrpvt`. **ADR/UDR devices only** diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index feb23f02..4d0e75c1 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(ublox_gps src/gnss.cpp src/gps.cpp src/hp_pos_rec_product.cpp + src/hpg_dr_product.cpp src/hpg_ref_product.cpp src/hpg_rov_product.cpp src/mkgmtime.c diff --git a/ublox_gps/config/zed_f9r.yaml b/ublox_gps/config/zed_f9r.yaml new file mode 100644 index 00000000..b196dde1 --- /dev/null +++ b/ublox_gps/config/zed_f9r.yaml @@ -0,0 +1,26 @@ +# Configuration Settings for the C102-F9R (ZED-F9R) device +ublox_gps_node: + ros__parameters: + config_on_startup: false # Sufficient device configuration not feasible + # with the current ublox node codebase. Use U-Center instead. + uart1: + baudrate: 115200 + in: 1 # ubx protocol id, see CfgPRT.msg + out: 0 # ubx protocol id, see CfgPRT.msg + + debug: 3 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: zedf9r + + inf: + all: false # Whether to display all INF messages in console + + publish: + all: false + esf: + raw: true + meas: true + nav: + att: true + hpposllh: true + pvt: true diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 4952a7c8..47704ebc 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -66,11 +66,14 @@ class AdrUdrProduct final : public virtual ComponentInterface { bool use_adr_; sensor_msgs::msg::Imu imu_; + sensor_msgs::msg::Imu imu_raw_; sensor_msgs::msg::TimeReference t_ref_; void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); + void callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m); rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr imu_raw_pub_; rclcpp::Publisher::SharedPtr time_ref_pub_; rclcpp::Publisher::SharedPtr nav_att_pub_; rclcpp::Publisher::SharedPtr esf_ins_pub_; diff --git a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp new file mode 100644 index 00000000..87293b62 --- /dev/null +++ b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp @@ -0,0 +1,123 @@ +#ifndef UBLOX_GPS_HPG_DR_PRODUCT_HPP +#define UBLOX_GPS_HPG_DR_PRODUCT_HPP + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +/** + * @brief High-Precision GNSS Dead Reckoning product interface for firmware >= 8 + */ +class HpgDrProduct final : public virtual ComponentInterface { + public: + explicit HpgDrProduct(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 High-Precision GNSS DR messages. + */ + void subscribe(std::shared_ptr gps) override; + + + /** + * @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; + + void initializeRosDiagnostics() override { + // RCLCPP_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", + // "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); + } + + private: + + // Scaling factors for floating point representations of data + static constexpr float kConvertRadPerSec = std::pow(2, -12) * M_PI / 180.0F; + static constexpr float kConvertMPerSecSq = std::pow(2, -10); + + // Callbacks relevant to ROS2 message output triggered on U-Blox message events + void callbackEsfIns(const ublox_msgs::msg::EsfINS &m); + void callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m); + void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); + void callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m); + void callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH &m); + void callbackNavATT(const ublox_msgs::msg::NavATT &m); + void callbackNavPvt(const ublox_msgs::msg::NavPVT &m); + + // Reused containers for ROS2 messages + sensor_msgs::msg::Imu imu_meas_; + sensor_msgs::msg::Imu imu_raw_; + sensor_msgs::msg::Imu imu_att_; + sensor_msgs::msg::Imu esf_ins_ros_; + sensor_msgs::msg::NavSatFix fix_hp_; + diagnostic_msgs::msg::DiagnosticStatus nav_diag_; + + //! Last global position (used for diagnostic updater) + ublox_msgs::msg::NavHPPOSLLH last_hp_pos_; + + // ROS2 publishers for ROS2 messages + rclcpp::Publisher::SharedPtr imu_meas_pub_; + rclcpp::Publisher::SharedPtr imu_raw_pub_; + rclcpp::Publisher::SharedPtr imu_att_pub_; + rclcpp::Publisher::SharedPtr esf_ins_ros_pub_; + rclcpp::Publisher::SharedPtr fix_hp_pub_; + rclcpp::Publisher::SharedPtr nav_diag_pub_; + + // ROS2 republishers for U-Blox messages + rclcpp::Publisher::SharedPtr nav_att_pub_; + rclcpp::Publisher::SharedPtr nav_pvt_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_; + + + // Relevant configuration state parameters + bool use_adr_; + uint16_t nav_rate_; + uint16_t meas_rate_; + + std::string frame_id_; + + std::shared_ptr updater_; + std::vector rtcms_; + rclcpp::Node* node_; + + // Local cache of U-Blox messages to be fused within a navigation epoch (data frame) + ublox_msgs::msg::NavATT last_nav_att_; + ublox_msgs::msg::NavPVT last_nav_pvt_; + std::pair last_itow_time_; + +}; + +} // namespace ublox_node + +#endif // UBLOX_GPS_HPG_DR_PRODUCT_HPP diff --git a/ublox_gps/include/ublox_gps/node.hpp b/ublox_gps/include/ublox_gps/node.hpp index e4d70575..6d3a4d22 100644 --- a/ublox_gps/include/ublox_gps/node.hpp +++ b/ublox_gps/include/ublox_gps/node.hpp @@ -191,7 +191,8 @@ class UbloxNode final : public rclcpp::Node { * products this string is empty */ void addProductInterface(const std::string & product_category, - const std::string & ref_rov = ""); + const std::string & ref_rov = "", + const std::string & product_model = ""); /** * @brief Poll version message from the U-Blox device to keep socket active. @@ -236,6 +237,8 @@ class UbloxNode final : public rclcpp::Node { uint16_t uart_out_{0}; //! USB TX Ready Pin configuration (see CfgPRT message for constants) uint16_t usb_tx_{0}; + //! Whether to override-configure the U-Blox the device on node startup + bool config_on_startup_{true}; //! Whether to configure the USB port /*! Set to true if usb_in & usb_out parameters are set */ bool set_usb_{false}; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index e5bbc895..90c0d788 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -79,6 +79,10 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { rclcpp::Publisher::SharedPtr nav_sat_pub_; rclcpp::Publisher::SharedPtr mon_hw_pub_; rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; +<<<<<<< HEAD +======= + +>>>>>>> 0cb2c06 (Add full product support for ZED-F9R) }; } // namespace ublox_node diff --git a/ublox_gps/include/ublox_gps/utils.hpp b/ublox_gps/include/ublox_gps/utils.hpp index 0e535b0d..b6fc1a8e 100644 --- a/ublox_gps/include/ublox_gps/utils.hpp +++ b/ublox_gps/include/ublox_gps/utils.hpp @@ -96,6 +96,7 @@ template bool getRosUint(rclcpp::Node* node, const std::string& key, U &u) { rclcpp::Parameter parameter; if (!node->get_parameter(key, parameter)) { + RCLCPP_WARN(node->get_logger(), "Failed to get ROS2 node parameter %s", key.c_str()); return false; } U param = parameter.get_value(); diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 0035b20c..2dcc9a32 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -22,6 +22,20 @@ namespace ublox_node { +// Scaling factors for floating point representations of data +constexpr float kConvertRadPerSec{std::pow(2, -12) * M_PI / 180.0F}; +constexpr float kConvertMps2{std::pow(2, -10)}; +constexpr float kConvertDegCelsius{0.01F}; + +// +// Extract U-Blox 24-bit signed integers from a 32-bit data blob +// and cast into int32_t +// +static inline std::int32_t extract_int24(std::uint32_t bitfield) { + std::int32_t temp = static_cast(bitfield << 8); + return temp >> 8; +} + // // u-blox ADR devices, partially implemented // @@ -33,7 +47,6 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s 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")) { @@ -44,6 +57,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s } if (getRosBoolean(node_, "publish.esf.raw")) { esf_raw_pub_ = node_->create_publisher("esfraw", 1); + imu_raw_pub_ = node_->create_publisher("imu_raw", 1); } if (getRosBoolean(node_, "publish.esf.status")) { esf_status_pub_ = node_->create_publisher("esfstatus", 1); @@ -87,7 +101,6 @@ 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); @@ -97,6 +110,10 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { if (getRosBoolean(node_, "publish.esf.raw")) { gps->subscribe([this](const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); }, 1); + + // also publish sensor_msgs::Imu for the raw data + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); } // Subscribe to ESF Status messages @@ -116,92 +133,128 @@ 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_; + imu_.orientation_covariance[0] = -1; + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; - 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; - for (unsigned int datapoint : imu_data) { + const std::vector imu_data = m.data; + for (const std::uint32_t 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; - } else { - data_sign = 1; - } + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + break; + imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); + break; // Reserved for future use + // The following are not currently used + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; - // 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; - imu_.angular_velocity_covariance[0] = -1; - - if (data_type == 14) { - if (data_sign == 1) { - imu_.angular_velocity.x = 2048 - data_value * rad_per_sec; - } else { - 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); - //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 { - 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 * rad_per_sec; - } else { - imu_.angular_velocity.y = data_sign * data_value * rad_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 * rad_per_sec; - } else { - imu_.angular_velocity.z = data_sign * data_value * rad_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) { - // RCLCPP_INFO("Temperature in celsius: %f", data_value * deg_c); - } else { - RCLCPP_INFO(node_->get_logger(), "data_type: %u", data_type); - RCLCPP_INFO(node_->get_logger(), "data_value: %u", data_value); + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); } // create time ref message and put in the data - //t_ref_.header.seq = m.risingEdgeCount; - //t_ref_.header.stamp = node_->now(); - //t_ref_.header.frame_id = frame_id_; + t_ref_.header.seq = m.risingEdgeCount; + t_ref_.header.stamp = node_->now(); + t_ref_.header.frame_id = frame_id_; - //t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + // Consider removing the below + t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + time_ref_pub_->publish(t_ref_); + imu_pub_->publish(imu_); + + // Consider removing the below //std::ostringstream src; //src << "TIM" << int(m.ch); //t_ref_.source = src.str(); + } + } - 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_); +void AdrUdrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { + if (getRosBoolean(node_, "publish.esf.raw")) { + imu_raw_.header.stamp = node_->now(); + imu_raw_.header.frame_id = frame_id_; + imu_raw_.orientation_covariance[0] = -1; + imu_raw_.linear_acceleration_covariance[0] = -1; + imu_raw_.angular_velocity_covariance[0] = -1; + + const std::vector imu_data_blocks = m.blocks; + for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : imu_data_blocks) { + const std::uint32_t datapoint = imu_data_entry.data; + + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer, and cast to double + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + break; // Reserved for future use + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; // Reserved for future use + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); + } } + imu_raw_pub_->publish(imu_raw_); } - } +>>>>>>> 0cb2c06 (Add full product support for ZED-F9R) } // namespace ublox_node diff --git a/ublox_gps/src/hpg_dr_product.cpp b/ublox_gps/src/hpg_dr_product.cpp new file mode 100644 index 00000000..7bda1cb2 --- /dev/null +++ b/ublox_gps/src/hpg_dr_product.cpp @@ -0,0 +1,417 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ublox_node { + +// +// Extract U-Blox 24-bit signed integers from a 32-bit data blob +// and cast into int32_t +// +static inline std::int32_t extract_int24(std::uint32_t data) { + const std::uint8_t* b = reinterpret_cast(&data); + return (std::int32_t)( + (((std::uint32_t)b[2] << 24) & 0xFF000000) | + (((std::uint32_t)b[1] << 16) & 0x00FF0000) | + (((std::uint32_t)b[0] << 8) & 0x0000FF00) + ) / 256; +} + +// +// U-Blox High Precision GNSS product with Dead Reckoning +// These appear to only have firmware version >= 8 +// +HpgDrProduct::HpgDrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) + : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), rtcms_(rtcms), node_(node) +{ + if (getRosBoolean(node_, "publish.esf.meas")) { + 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.nav.pvt")) { + nav_pvt_pub_ = node_->create_publisher("navpvt", 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); + } + + imu_meas_pub_ = node_->create_publisher("~/imu_meas", 1); + imu_att_pub_ = node_->create_publisher("~/imu_att", 1); + esf_ins_ros_pub_ = node_->create_publisher("~/veh_kinematics", 1); + imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); + fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); + esf_diag_pub_ = node_->create_publisher("~/fusion_status", 1); + + // Perform any message metadata value setting we can do only once, including default values + // This only improves performance a little, but removes duplcate code + imu_meas_.header.frame_id = frame_id_; + + imu_raw_.header.frame_id = frame_id_; + imu_raw_.orientation_covariance[0] = -1; + imu_raw_.linear_acceleration_covariance[0] = -1; + imu_raw_.angular_velocity_covariance[0] = -1; + + imu_att_.header.frame_id = frame_id_; + imu_att_.linear_acceleration_covariance[0] = -1; // signifies missing data + imu_att_.angular_velocity_covariance[0] = -1; // signifies missing data + + esf_ins_ros_.header.frame_id = frame_id_; + esf_ins_ros_.linear_acceleration_covariance[0] = -1; // signifies missing data + esf_ins_ros_.angular_velocity_covariance[0] = -1; // signifies missing data + + fix_hp_.header.frame_id = frame_id_; +} + +void HpgDrProduct::subscribe(std::shared_ptr gps) { + // Subscribe to ADR/UDR Navigation Attitude messages + gps->subscribe(std::bind( + &HpgDrProduct::callbackNavATT, this, std::placeholders::_1), 1); + + gps->subscribe(std::bind( + &HpgDrProduct::callbackNavPvt, this, std::placeholders::_1), 1); + + // ESF status for diagnostics + gps->subscribe(std::bind( + &HpgDrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); + + // Subscribe to High-Precision Geodetic Position messages + gps->subscribe(std::bind( + &HpgDrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); + + // Subscribe to ADR/UDR Post-Processed IMU messages + gps->subscribe(std::bind( + &HpgDrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); + + // Subscribe to ADR/UDR Inertial Navigation System kinematics messages + gps->subscribe(std::bind( + &HpgDrProduct::callbackEsfIns, this, std::placeholders::_1), 1); + + // Subscribe to ADR/UDR Raw IMU messages + gps->subscribe(std::bind( + &HpgDrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); +} + +void HpgDrProduct::getRosParams() { + use_adr_ = getRosBoolean(node_, "use_adr"); + // Check the nav rate + float nav_rate_hz = 1000.0 / (meas_rate_ * nav_rate_); + if (nav_rate_hz != 1) { + RCLCPP_WARN(node_->get_logger(), "ADR/UDR Nav Rate recommended to be 1 Hz"); + } +} + +bool HpgDrProduct::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 HpgDrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { + imu_att_.header.stamp = node_->now(); + + if (getRosBoolean(node_, "publish.nav.att")) { + nav_att_pub_->publish(m); + } + + constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + + // Transform U-Blox Euler angles to Quaternion and populate covariances + const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); + tf2::Quaternion orientation; + orientation.setRPY(roll, pitch, heading); + + imu_att_.orientation.x = orientation[0]; + imu_att_.orientation.y = orientation[1]; + imu_att_.orientation.z = orientation[2]; + imu_att_.orientation.w = orientation[3]; + + imu_att_.orientation_covariance[0] = std::pow(m.acc_roll * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[4] = std::pow(m.acc_pitch * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[8] = std::pow(m.acc_heading * kNavAttScaleAndRadianConversion, 2); + + imu_att_pub_->publish(imu_att_); + last_nav_att_ = m; +} + +void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { + esf_ins_ros_.header.stamp = node_->now(); + + if (getRosBoolean(node_, "publish.esf.ins")) { + esf_ins_pub_->publish(m); + } + + // To avoid mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID + ublox_msgs::msg::NavATT temp_att = last_nav_att_; + // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT + if (temp_att.i_tow == m.i_tow) { + esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; + esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; + + constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + + const double roll = M_PI_2 - (static_cast(temp_att.roll) * kNavAttScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(temp_att.pitch) * kNavAttScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(temp_att.heading) * kNavAttScaleAndRadianConversion); + tf2::Quaternion orientation; + orientation.setRPY(roll, pitch, heading); + + esf_ins_ros_.orientation.x = orientation[0]; + esf_ins_ros_.orientation.y = orientation[1]; + esf_ins_ros_.orientation.z = orientation[2]; + esf_ins_ros_.orientation.w = orientation[3]; + + esf_ins_ros_.orientation_covariance[0] = std::pow(temp_att.acc_roll * kNavAttScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[4] = std::pow(temp_att.acc_pitch * kNavAttScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[8] = std::pow(temp_att.acc_heading * kNavAttScaleAndRadianConversion, 2); + } else { // No data available for this data frame + esf_ins_ros_.orientation_covariance[0] = -1; + esf_ins_ros_.orientation_covariance[4] = -1; + esf_ins_ros_.orientation_covariance[8] = -1; + } + + constexpr double kMilliGramsToNewtons{1e-6}; + constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; + + esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.x = static_cast(m.x_accel) * kMilliGramsToNewtons; + esf_ins_ros_.angular_velocity.y = static_cast(m.y_accel) * kMilliGramsToNewtons; + esf_ins_ros_.angular_velocity.z = static_cast(m.z_accel) * kMilliGramsToNewtons; + + esf_ins_ros_pub_->publish(esf_ins_ros_); +} + +void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { + std::uint8_t wheel_tick_status = m.reserved1[0] & 0xb00000011; + std::uint8_t imu_align_status = m.reserved1[0] & 0xb00011100; + std::uint8_t ins_init_status = m.reserved1[0] & 0xb01100000; + std::uint8_t imu_init_status = m.reserved1[1] & 0xb00000011; + + diagnostic_msgs::msg::KeyValue wt_status; + wt_status.key = "wheel_tick_status"; + wt_status.value = wheel_tick_status == 2 ? "initialized" : (wheel_tick_status == 1 ? "initializing" : "off"); + diagnostic_msgs::msg::KeyValue imu_alg; + imu_alg.key = "IMU_alignment_status"; + imu_alg.value = imu_align_status > 1 ? "initialized" : (imu_align_status == 1 ? "initializing" : "off"); + diagnostic_msgs::msg::KeyValue ins_ini; + ins_ini.key = "INS_init_status"; + ins_ini.value = ins_init_status == 2 ? "initialized" : (ins_init_status == 1 ? "initializing" : "off"); + diagnostic_msgs::msg::KeyValue imu_ini; + imu_ini.key = "IMU_init_status"; + imu_ini.value = imu_init_status == 2 ? "initialized" : (ins_init_status == 1 ? "initializing" : "off"); + diagnostic_msgs::msg::KeyValue fusion_mode; + fusion_mode.key = "fusion_mode"; + fusion_mode.value = m.fusion_mode == 3 ? "disabled_fault" : (m.fusion_mode == 2 ? "suspended" : (m.fusion_mode == 1 ? "online" : "initializing")); + diagnostic_msgs::msg::KeyValue num_sens; + num_sens.key = "num_sensors"; + num_sens.value = std::to_string(m.num_sens); + + diagnostic_msgs::msg::DiagnosticStatus nav_diag; + nav_diag.name = "NavigationDiagnostics"; + nav_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + nav_diag.hardware_id = "ublox ZED-F9"; + + nav_diag.values.push_back(imu_alg); + nav_diag.values.push_back(imu_ini); + nav_diag.values.push_back(wt_status); + nav_diag.values.push_back(ins_ini); + nav_diag.values.push_back(fusion_mode); + nav_diag.values.push_back(num_sens); + nav_diag_pub_->publish(nav_diag); +} + +// +// Decode the Processed IMU measurement output +// +void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + imu_meas_.header.stamp = node_->now(); + + if (getRosBoolean(node_, "publish.esf.meas")) { + esf_meas_pub_->publish(m); + } + + imu_meas_.orientation_covariance[0] = -1; + imu_meas_.linear_acceleration_covariance[0] = -1; + imu_meas_.angular_velocity_covariance[0] = -1; + + const std::vector imu_data = m.data; + for (const std::uint32_t datapoint : imu_data) { + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_meas_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_meas_.linear_acceleration.x = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_meas_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_meas_.linear_acceleration.y = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_meas_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_meas_.linear_acceleration.z = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; // Do nothing, just catch + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); + } + imu_meas_pub_->publish(imu_meas_); + } +} + +// +// Decode the Raw IMU measurement output +// +void HpgDrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { + if (getRosBoolean(node_, "publish.esf.raw")) { + esf_raw_pub_->publish(m); + } + imu_raw_.header.stamp = node_->now(); + + const std::vector imu_data_blocks = m.blocks; + for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : imu_data_blocks) { + const std::uint32_t datapoint = imu_data_entry.data; + + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer, and cast to double + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMPerSecSq; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + break; // Do nothing, just catch + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; // Do nothing, just catch + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); + } + } + imu_raw_pub_->publish(imu_raw_); +} + +// +// Decode the High-Precision Geodetic Position message (HPPOSLLH) into NavSatFix +// +void HpgDrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { + // Do not publish invalid HPPOSLLH data + if (m.flags != 0) { + return; + } + fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device + + if (last_nav_pvt_.fix_type >= ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { + fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + } else { + fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + + // Calculate the high-precision lat, lon, alt values + fix_hp_.latitude = 1e-7 * (static_cast(m.lat) + (static_cast(m.lat_hp) * 1e-2)); + fix_hp_.longitude = 1e-7 * (static_cast(m.lon) + (static_cast(m.lon_hp) * 1e-2)); + fix_hp_.altitude = 1e-3 * (static_cast(m.height) + (static_cast(m.height_hp) * 1e-1)); + + // Populate the covariance data + const double var_h = std::pow(m.h_acc / 1000.0, 2); + const double var_v = std::pow(m.v_acc / 1000.0, 2); + fix_hp_.position_covariance[0] = var_h; + fix_hp_.position_covariance[4] = var_h; + fix_hp_.position_covariance[8] = var_v; + fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_hp_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + fix_hp_pub_->publish(fix_hp_); +} + +// +// Use NavPVT for some message fusion diagnostics data +// +void HpgDrProduct::callbackNavPvt(const ublox_msgs::msg::NavPVT& m) { + if (getRosBoolean(node_, "publish.nav.pvt")) { + nav_pvt_pub_->publish(m); + } + + // update the iTow 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 + last_itow_time_.first = m.i_tow; + if (m.nano < 0) { + last_itow_time_.second.sec = toUtcSeconds(m) - 1; + last_itow_time_.second.nanosec = static_cast(m.nano + 1e9); + } + else { + last_itow_time_.second.sec = toUtcSeconds(m); + last_itow_time_.second.nanosec = static_cast(m.nano); + } + } +} + + +} // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 85cd615e..9bc01032 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -218,27 +219,41 @@ void UbloxNode::addFirmwareInterface() { void UbloxNode::addProductInterface(const std::string & product_category, - const std::string & ref_rov) { + const std::string & ref_rov, + const std::string & product_model) { + // High-Precision GNSS reference station products if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "REF") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, updater_, rtcms_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision GNSS Reference Stations"); + // High-Precision GNSS rover product, relying on a reference station } else if ((product_category == "HPG" || product_category == "HPS") && ref_rov == "ROV") { components_.push_back(std::make_shared(nav_rate_, updater_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision GNSS Rovers"); + // High-Precision GNSS Sensor Fusion products with Dead Reckoning capability + } else if (product_category == "HPS" && (product_model == "ZED-F9R" || product_model == "ZED-F9K")) { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision Dead Reckoning products."); + // Any other High-Precision GNSS product, without sensor fusion } else if (product_category == "HPG" || product_category == "HPS") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision GNSS products"); + // Time Sync products } else if (product_category == "TIM") { components_.push_back(std::make_shared(frame_id_, updater_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for Time Sync products"); + // Automotive Dead Reckoning or Untethered Dead Reckoning products not detected above } else if (product_category == "ADR" || product_category == "UDR") { components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, this)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for Automotive/Untethered Dead Reckoning products"); + // Frequency & other Time Synchronization products } 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)); + RCLCPP_INFO(this->get_logger(), "Registered product interface for Frequency & Time Synchronization products"); } 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"); + RCLCPP_WARN(this->get_logger(), "Product category %s %s (model %s) from MonVER message not recognized. %s", + product_category.c_str(), ref_rov.c_str(), product_model.c_str(), + "Options are HPG REF, HPG ROV, HPG #.#, TIM, ADR, UDR, FTS, HPS"); } } @@ -415,6 +430,7 @@ void UbloxNode::getRosParams() { 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.hpposllh", 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")); @@ -687,6 +703,9 @@ void UbloxNode::processMonVer() { gnss_->add(str); } } else { + std::string product_category{""}; // To be populated + std::string rov_ref{""}; // To be populated, if needed + std::string product_model{""}; for (std::size_t i = 0; i < extensions.size(); ++i) { std::vector strs; // Up to 2nd to last line @@ -694,10 +713,9 @@ void UbloxNode::processMonVer() { strs = stringSplit(extensions[i], "="); if (strs.size() > 1) { if (strs[0] == "FWVER") { + product_category = strs[1].substr(0, 3); if (strs[1].length() > 8) { - addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10)); - } else { - addProductInterface(strs[1].substr(0, 3)); + rov_ref = strs[1].substr(8, 10); } continue; } @@ -706,6 +724,7 @@ void UbloxNode::processMonVer() { { std::vector moduleField; moduleField = stringSplit(strs[1], "-"); + product_model = strs[1]; if (moduleField.size() > 1) { if (moduleField[1].substr(0,2) == "F9") @@ -728,6 +747,7 @@ void UbloxNode::processMonVer() { } } } + addProductInterface(product_category, rov_ref, product_model); } } @@ -907,8 +927,12 @@ void UbloxNode::initialize() { // Do this last initializeRosDiagnostics(); - if (configureUblox()) { - RCLCPP_INFO(this->get_logger(), "U-Blox configured successfully."); + if (!getRosBoolean(this, "config_on_startup") || configureUblox()) { + if (getRosBoolean(this, "config_on_startup")) { + RCLCPP_INFO(this->get_logger(), "U-Blox device configured successfully."); + } else { + RCLCPP_INFO(this->get_logger(), "U-Blox device configuration not modified."); + } // Subscribe to all U-Blox messages subscribe(); // Configure INF messages (needs INF params, call after subscribing) diff --git a/ublox_mcu-config b/ublox_mcu-config new file mode 100644 index 00000000..20d34b70 --- /dev/null +++ b/ublox_mcu-config @@ -0,0 +1,8 @@ +# Wheel speed RR +0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x27 0x10 0x91 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x39 0x01 0xe7 0x2d + +# Wheel speed RL +0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x37 0x10 0x92 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x29 0x01 0xe8 0xd7 + +# Direction (Drive gear engaged or not) +0x43 0xa2 0x11 0x13 0x03 0xbc 0x03 0x00 0x00 0x08 0x64 0x2f 0x01 0x00 0x00 0x01 0x00 0x00 0x00 0xe8 0x03 0x5f 0x01 0xce 0xb8 \ No newline at end of file diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 4e3db305..339fa44a 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -66,6 +66,7 @@ set(msg_files "msg/NavDGPS.msg" "msg/NavDGPSSV.msg" "msg/NavDOP.msg" + "msg/NavHPPOSLLH.msg" "msg/NavPOSECEF.msg" "msg/NavPOSLLH.msg" "msg/NavPVT7.msg" diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 14418e39..d7820799 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1756,6 +1756,52 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavHPPOSLLH_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + 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.lon_hp); + stream.next(m.lat_hp); + stream.next(m.height_hp); + stream.next(m.h_msl_hp); + stream.next(m.h_acc); + stream.next(m.v_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavHPPOSLLH_ & m) { + (void)m; + return 36; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavHPPOSLLH_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved1); + stream.next(m.flags); + 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.lon_hp); + stream.next(m.lat_hp); + stream.next(m.height_hp); + stream.next(m.h_msl_hp); + stream.next(m.h_acc); + stream.next(m.v_acc); + } +}; + 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 b1ac7155..1678e2b5 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -162,6 +163,7 @@ namespace Message { 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 HPPOSLLH = ublox_msgs::msg::NavHPPOSLLH::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; diff --git a/ublox_msgs/msg/EsfMEAS.msg b/ublox_msgs/msg/EsfMEAS.msg index 24017c76..0ed49c91 100644 --- a/ublox_msgs/msg/EsfMEAS.msg +++ b/ublox_msgs/msg/EsfMEAS.msg @@ -30,7 +30,7 @@ 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_GYRO_ANG_RATE_Z = 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. diff --git a/ublox_msgs/msg/NavHPPOSLLH.msg b/ublox_msgs/msg/NavHPPOSLLH.msg new file mode 100644 index 00000000..73f33851 --- /dev/null +++ b/ublox_msgs/msg/NavHPPOSLLH.msg @@ -0,0 +1,32 @@ +# NAV-HPPOSLLH (0x01 0x14) +# High-Precision Geodetic Position Solution +# +# This message is available in +# +# High-Precision longitude = lon + (lon_hp * 1e-2) +# High-Precision latitude = lat + (lat_hp * 1e-2) +# High-Precision altitude(ell) = height + (height_hp * 0.1) +# High-Precision altitude(msl) = h_msl + h_msl_hp * 0.1) + +uint8 CLASS_ID = 0x01 +uint8 MESSAGE_ID = 0x14 + + +uint8 version +uint16 reserved1 +uint8 flags # bit 0: 1=invalid position, 0 = valid + +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 h_msl # Height above mean sea level [mm] + +int8 lon_hp # High-prec. longitude component +int8 lat_hp # High-prec. latitude component +int8 height_hp # High-Prec. ellpisoid altitude component +int8 h_msl_hp # High-Prec. MSL altitude component + +uint32 h_acc # Horizontal Accuracy Estimate [mm] +uint32 v_acc # Vertical Accuracy Estimate [mm] diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index ac905307..6a3d1736 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -39,11 +39,13 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::ATT, 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) + 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, ublox_msgs, NavDOP) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, + ublox_msgs, NavHPPOSLLH) 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, diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index f86baddd..0617d23b 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include From 6cc9a2764b5ad24fe488ed487413683ab75907ce Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Wed, 24 Jan 2024 11:48:17 +0000 Subject: [PATCH 02/13] Add support for building on older ROS2 distros, minor bugfixes * Add missing include directory directives in `ublox_gps/CMakeLists.txt` for ublox_msgs and ublox_serialization packages * Add ROSIDL cmake interface agnosticity between older and newer ROS2 distros in `ublox_msgs/CMakeLists.txt` * Fix satellite fix status classification in `ublox_gps/src/hpg_dr_product.cpp` * Add missing debug publisher for NavHPPOSLLH --- ublox_gps/CMakeLists.txt | 2 + .../include/ublox_gps/hpg_dr_product.hpp | 1 + ublox_gps/src/hpg_dr_product.cpp | 110 ++++++++++++------ ublox_msgs/CMakeLists.txt | 69 ++++++----- 4 files changed, 122 insertions(+), 60 deletions(-) diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index 4d0e75c1..f61265a4 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -51,6 +51,8 @@ target_include_directories(ublox_gps PUBLIC "$" "$" ${diagnostic_updater_INCLUDE_DIRS} + ${ublox_msgs_INCLUDE_DIRS} + ${ublox_serialization_INCLUDE_DIRS} ) target_link_libraries(ublox_gps PUBLIC ${asio_LIBRARIES} diff --git a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp index 87293b62..3cb887da 100644 --- a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp @@ -93,6 +93,7 @@ class HpgDrProduct final : public virtual ComponentInterface { // ROS2 republishers for U-Blox messages rclcpp::Publisher::SharedPtr nav_att_pub_; + rclcpp::Publisher::SharedPtr nav_hpposllh_pub_; rclcpp::Publisher::SharedPtr nav_pvt_pub_; rclcpp::Publisher::SharedPtr esf_ins_pub_; rclcpp::Publisher::SharedPtr esf_meas_pub_; diff --git a/ublox_gps/src/hpg_dr_product.cpp b/ublox_gps/src/hpg_dr_product.cpp index 7bda1cb2..addf7bbd 100644 --- a/ublox_gps/src/hpg_dr_product.cpp +++ b/ublox_gps/src/hpg_dr_product.cpp @@ -60,7 +60,7 @@ HpgDrProduct::HpgDrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::str esf_ins_ros_pub_ = node_->create_publisher("~/veh_kinematics", 1); imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); - esf_diag_pub_ = node_->create_publisher("~/fusion_status", 1); + nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); // Perform any message metadata value setting we can do only once, including default values // This only improves performance a little, but removes duplcate code @@ -129,32 +129,36 @@ bool HpgDrProduct::configureUblox(std::shared_ptr gps) { } void HpgDrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { + ublox_msgs::msg::NavATT temp_att = last_nav_att_; + if (temp_att.i_tow == m.i_tow) { + esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; + esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; imu_att_.header.stamp = node_->now(); + } + if (getRosBoolean(node_, "publish.nav.att")) { + nav_att_pub_->publish(m); + } - if (getRosBoolean(node_, "publish.nav.att")) { - nav_att_pub_->publish(m); - } - - constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - // Transform U-Blox Euler angles to Quaternion and populate covariances - const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); - const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); - const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); - tf2::Quaternion orientation; - orientation.setRPY(roll, pitch, heading); + // Transform U-Blox Euler angles to Quaternion and populate covariances + const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); + tf2::Quaternion orientation; + orientation.setRPY(roll, pitch, heading); - imu_att_.orientation.x = orientation[0]; - imu_att_.orientation.y = orientation[1]; - imu_att_.orientation.z = orientation[2]; - imu_att_.orientation.w = orientation[3]; + imu_att_.orientation.x = orientation[0]; + imu_att_.orientation.y = orientation[1]; + imu_att_.orientation.z = orientation[2]; + imu_att_.orientation.w = orientation[3]; - imu_att_.orientation_covariance[0] = std::pow(m.acc_roll * kNavAttScaleAndRadianConversion, 2); - imu_att_.orientation_covariance[4] = std::pow(m.acc_pitch * kNavAttScaleAndRadianConversion, 2); - imu_att_.orientation_covariance[8] = std::pow(m.acc_heading * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[0] = std::pow(m.acc_roll * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[4] = std::pow(m.acc_pitch * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[8] = std::pow(m.acc_heading * kNavAttScaleAndRadianConversion, 2); - imu_att_pub_->publish(imu_att_); - last_nav_att_ = m; + imu_att_pub_->publish(imu_att_); + last_nav_att_ = m; } void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { @@ -164,13 +168,10 @@ void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); } - // To avoid mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID + // To avoid long mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID ublox_msgs::msg::NavATT temp_att = last_nav_att_; // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT if (temp_att.i_tow == m.i_tow) { - esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; - esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; - constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; const double roll = M_PI_2 - (static_cast(temp_att.roll) * kNavAttScaleAndRadianConversion); @@ -206,24 +207,53 @@ void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_ros_pub_->publish(esf_ins_ros_); } -void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { - std::uint8_t wheel_tick_status = m.reserved1[0] & 0xb00000011; - std::uint8_t imu_align_status = m.reserved1[0] & 0xb00011100; - std::uint8_t ins_init_status = m.reserved1[0] & 0xb01100000; - std::uint8_t imu_init_status = m.reserved1[1] & 0xb00000011; +const char* wt_status_str(std::uint8_t bitfield) { + std::uint8_t wt_status = bitfield & 0xb00000011; + if (wt_status == 2) return "calibrated"; + if (wt_status == 1) return "initializing"; + if (wt_status == 0) return "off"; + return "deserialization error"; +} + +const char* imu_alg_str(std::uint8_t bitfield) { + std::uint8_t alg_status = bitfield & 0xb00011100; + if (alg_status == 2) return "calibrated"; + if (alg_status == 1) return "initializing"; + if (alg_status == 0) return "off"; + return "deserialization error"; +} + +const char* ins_init_str(std::uint8_t bitfield) { + std::uint8_t ins_status = bitfield & 0xb01100000; + if (ins_status == 2) return "calibrated"; + if (ins_status == 1) return "initializing"; + if (ins_status == 0) return "off"; + return "deserialization error"; +} + +const char* imu_init_str(std::uint8_t bitfield) { + std::uint8_t imu_status = bitfield & 0xb00000011; + if (imu_status == 2) return "calibrated"; + if (imu_status == 1) return "initializing"; + if (imu_status == 0) return "off"; + return "deserialization error"; +} + + +void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { diagnostic_msgs::msg::KeyValue wt_status; wt_status.key = "wheel_tick_status"; - wt_status.value = wheel_tick_status == 2 ? "initialized" : (wheel_tick_status == 1 ? "initializing" : "off"); + wt_status.value = wt_status_str(m.reserved1[0]); diagnostic_msgs::msg::KeyValue imu_alg; imu_alg.key = "IMU_alignment_status"; - imu_alg.value = imu_align_status > 1 ? "initialized" : (imu_align_status == 1 ? "initializing" : "off"); + imu_alg.value = imu_alg_str(m.reserved1[0]); diagnostic_msgs::msg::KeyValue ins_ini; ins_ini.key = "INS_init_status"; - ins_ini.value = ins_init_status == 2 ? "initialized" : (ins_init_status == 1 ? "initializing" : "off"); + ins_ini.value = ins_init_str(m.reserved1[0]); diagnostic_msgs::msg::KeyValue imu_ini; imu_ini.key = "IMU_init_status"; - imu_ini.value = imu_init_status == 2 ? "initialized" : (ins_init_status == 1 ? "initializing" : "off"); + imu_ini.value = imu_init_str(m.reserved1[1]); diagnostic_msgs::msg::KeyValue fusion_mode; fusion_mode.key = "fusion_mode"; fusion_mode.value = m.fusion_mode == 3 ? "disabled_fault" : (m.fusion_mode == 2 ? "suspended" : (m.fusion_mode == 1 ? "online" : "initializing")); @@ -282,16 +312,22 @@ void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_FL %d\n", data_value); break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_FR %d\n", data_value); break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_RL %d\n", data_value); break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_RR %d\n", data_value); break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_SINGLE %d\n", data_value); break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_SPEED %d\n", data_value); break; // Do nothing, just catch default: RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); @@ -360,6 +396,11 @@ void HpgDrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { if (m.flags != 0) { return; } + + if (getRosBoolean(node_, "publish.nav.hpposllh")) { + nav_hpposllh_pub_->publish(m); + } + fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device if (last_nav_pvt_.fix_type >= ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { @@ -392,6 +433,7 @@ void HpgDrProduct::callbackNavPvt(const ublox_msgs::msg::NavPVT& m) { if (getRosBoolean(node_, "publish.nav.pvt")) { nav_pvt_pub_->publish(m); } + last_nav_pvt_ = m; // update the iTow timestamp uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED; diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 339fa44a..040dfae6 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(ublox_msgs) +set(LEGACY_ROS_DISTROS "foxy" "galactic" "eloquent" "dashing" "crystal" "bouncy" "ardent" "beta3" "beta2" "beta1") + # Default to C++14 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -10,7 +12,9 @@ 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_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -102,39 +106,52 @@ set(msg_files "msg/UpdSOS.msg" ) +if(NOT "$ENV{ROS_DISTRO}" IN_LIST LEGACY_ROS_DISTROS) + set(USE_LEGACY_ROSIDL_TARGET "OFF") +else() + message(STATUS "Using legacy rosidl cmake target interface") + set(USE_LEGACY_ROSIDL_TARGET "ON") +endif() + +add_library(${PROJECT_NAME}_lib src/ublox_msgs.cpp) +target_include_directories(${PROJECT_NAME}_lib PRIVATE + "$" + "$") + rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} + LIBRARY_NAME ${PROJECT_NAME} DEPENDENCIES sensor_msgs - std_msgs -) + std_msgs) -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(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) +if(USE_LEGACY_ROSIDL_TARGET) + rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") endif() +add_dependencies(${PROJECT_NAME}_lib ${PROJECT_NAME}) + +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(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) + ament_export_dependencies(rosidl_default_runtime sensor_msgs std_msgs ublox_serialization) ament_package() From 94b9e1fe3ce4fc57fd52ed39786b4c4afcb8bad2 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 18:04:42 -0800 Subject: [PATCH 03/13] Merge HPG DR into AdrUdrProduct --- ublox_gps/CMakeLists.txt | 1 - .../include/ublox_gps/adr_udr_product.hpp | 49 +- .../include/ublox_gps/hpg_dr_product.hpp | 10 +- .../include/ublox_gps/ublox_firmware8.hpp | 4 - ublox_gps/src/adr_udr_product.cpp | 549 +++++++++++++----- ublox_gps/src/hpg_dr_product.cpp | 178 ++++-- ublox_gps/src/node.cpp | 5 +- ublox_mcu-config | 8 - 8 files changed, 565 insertions(+), 239 deletions(-) delete mode 100644 ublox_mcu-config diff --git a/ublox_gps/CMakeLists.txt b/ublox_gps/CMakeLists.txt index f61265a4..281df504 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -34,7 +34,6 @@ add_library(ublox_gps src/gnss.cpp src/gps.cpp src/hp_pos_rec_product.cpp - src/hpg_dr_product.cpp src/hpg_ref_product.cpp src/hpg_rov_product.cpp src/mkgmtime.c diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 47704ebc..5fb5710b 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -3,11 +3,17 @@ #include #include +#include + +#include +#include #include #include #include +#include #include +#include #include #include @@ -15,20 +21,27 @@ #include #include #include +#include #include -#include #include +#include namespace ublox_node { +// Scaling factors for floating point representations of data +constexpr float kConvertRadPerSec{std::pow(2, -12) * M_PI / 180.0F}; +constexpr float kConvertMps2{std::pow(2, -10)}; +constexpr float kConvertDegCelsius{0.01F}; + /** * @brief Implements functions for Automotive Dead Reckoning (ADR) and - * Untethered Dead Reckoning (UDR) Devices. + * Untethered Dead Reckoning (UDR) Devices. High-Precision model is selected + * on the basis of the product model information. */ 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(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const std::string & product_model, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -67,15 +80,42 @@ class AdrUdrProduct final : public virtual ComponentInterface { sensor_msgs::msg::Imu imu_; sensor_msgs::msg::Imu imu_raw_; + sensor_msgs::msg::Imu imu_att_; + sensor_msgs::msg::Imu esf_ins_ros_; + sensor_msgs::msg::Temperature imu_temp_; sensor_msgs::msg::TimeReference t_ref_; + sensor_msgs::msg::NavSatFix fix_hp_; + diagnostic_msgs::msg::DiagnosticStatus nav_diag_; + ublox_msgs::msg::NavHPPOSLLH last_hp_pos_; - void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); + // Local cache of U-Blox messages to be fused within a navigation epoch (data frame) + ublox_msgs::msg::NavATT last_nav_att_; + ublox_msgs::msg::NavPVT last_nav_pvt_; + std::pair last_itow_time_; + + // Callbacks relevant to ROS2 message output triggered on U-Blox message events + void callbackEsfIns(const ublox_msgs::msg::EsfINS &m); void callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m); + void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); + void callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m); + void callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH &m); + void callbackNavATT(const ublox_msgs::msg::NavATT &m); + void callbackNavPVT(const ublox_msgs::msg::NavPVT &m); + // Publishers for ROS2 messages rclcpp::Publisher::SharedPtr imu_pub_; rclcpp::Publisher::SharedPtr imu_raw_pub_; + rclcpp::Publisher::SharedPtr imu_att_pub_; + rclcpp::Publisher::SharedPtr esf_ins_ros_pub_; + rclcpp::Publisher::SharedPtr imu_temp_pub_; + rclcpp::Publisher::SharedPtr fix_hp_pub_; + rclcpp::Publisher::SharedPtr nav_diag_pub_; rclcpp::Publisher::SharedPtr time_ref_pub_; + + // Republishers for U-Blox messages rclcpp::Publisher::SharedPtr nav_att_pub_; + rclcpp::Publisher::SharedPtr nav_pvt_pub_; + rclcpp::Publisher::SharedPtr nav_hpposllh_pub_; rclcpp::Publisher::SharedPtr esf_ins_pub_; rclcpp::Publisher::SharedPtr esf_meas_pub_; rclcpp::Publisher::SharedPtr esf_raw_pub_; @@ -87,6 +127,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { std::string frame_id_; std::shared_ptr updater_; + std::string product_model_; rclcpp::Node* node_; }; diff --git a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp index 3cb887da..6f69c2ac 100644 --- a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp +++ b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -60,8 +61,9 @@ class HpgDrProduct final : public virtual ComponentInterface { private: // Scaling factors for floating point representations of data - static constexpr float kConvertRadPerSec = std::pow(2, -12) * M_PI / 180.0F; - static constexpr float kConvertMPerSecSq = std::pow(2, -10); + //static constexpr float kConvertRadPerSec = std::pow(2, -12) * M_PI / 180.0F; + //static constexpr float kConvertMPerSecSq = std::pow(2, -10); + //static constexpr float kConvertDegCelsius{0.01F}; // Callbacks relevant to ROS2 message output triggered on U-Blox message events void callbackEsfIns(const ublox_msgs::msg::EsfINS &m); @@ -77,6 +79,8 @@ class HpgDrProduct final : public virtual ComponentInterface { sensor_msgs::msg::Imu imu_raw_; sensor_msgs::msg::Imu imu_att_; sensor_msgs::msg::Imu esf_ins_ros_; + sensor_msgs::msg::Temperature imu_temp_; + sensor_msgs::msg::NavSatFix fix_hp_; diagnostic_msgs::msg::DiagnosticStatus nav_diag_; @@ -87,6 +91,7 @@ class HpgDrProduct final : public virtual ComponentInterface { rclcpp::Publisher::SharedPtr imu_meas_pub_; rclcpp::Publisher::SharedPtr imu_raw_pub_; rclcpp::Publisher::SharedPtr imu_att_pub_; + rclcpp::Publisher::SharedPtr imu_temp_pub_; rclcpp::Publisher::SharedPtr esf_ins_ros_pub_; rclcpp::Publisher::SharedPtr fix_hp_pub_; rclcpp::Publisher::SharedPtr nav_diag_pub_; @@ -100,7 +105,6 @@ class HpgDrProduct final : public virtual ComponentInterface { rclcpp::Publisher::SharedPtr esf_raw_pub_; rclcpp::Publisher::SharedPtr esf_status_pub_; - // Relevant configuration state parameters bool use_adr_; uint16_t nav_rate_; diff --git a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp index 90c0d788..e5bbc895 100644 --- a/ublox_gps/include/ublox_gps/ublox_firmware8.hpp +++ b/ublox_gps/include/ublox_gps/ublox_firmware8.hpp @@ -79,10 +79,6 @@ class UbloxFirmware8 : public UbloxFirmware7Plus { rclcpp::Publisher::SharedPtr nav_sat_pub_; rclcpp::Publisher::SharedPtr mon_hw_pub_; rclcpp::Publisher::SharedPtr rxm_rtcm_pub_; -<<<<<<< HEAD -======= - ->>>>>>> 0cb2c06 (Add full product support for ZED-F9R) }; } // namespace ublox_node diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 2dcc9a32..2bbc4117 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -8,6 +9,7 @@ #include #include #include +#include #include #include @@ -22,11 +24,6 @@ namespace ublox_node { -// Scaling factors for floating point representations of data -constexpr float kConvertRadPerSec{std::pow(2, -12) * M_PI / 180.0F}; -constexpr float kConvertMps2{std::pow(2, -10)}; -constexpr float kConvertDegCelsius{0.01F}; - // // Extract U-Blox 24-bit signed integers from a 32-bit data blob // and cast into int32_t @@ -37,10 +34,11 @@ static inline std::int32_t extract_int24(std::uint32_t bitfield) { } // -// u-blox ADR devices, partially implemented +// U-Blox Automotive or Untethered Dead Reckoning +// High Precision GNSS products have have firmware version >= 8 // -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(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const std::string & product_model, rclcpp::Node* node) + : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node), product_model_(product_model) { if (getRosBoolean(node_, "publish.esf.meas")) { imu_pub_ = @@ -52,12 +50,17 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s if (getRosBoolean(node_, "publish.nav.att")) { nav_att_pub_ = node_->create_publisher("navatt", 1); } + if (getRosBoolean(node_, "publish.nav.hpposllh")) { + nav_hpposllh_pub_ = node_->create_publisher("navhpposllh", 1); + } + if (getRosBoolean(node_, "publish.nav.pvt")) { + nav_pvt_pub_ = node_->create_publisher("navpvt", 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); - imu_raw_pub_ = node_->create_publisher("imu_raw", 1); } if (getRosBoolean(node_, "publish.esf.status")) { esf_status_pub_ = node_->create_publisher("esfstatus", 1); @@ -65,8 +68,105 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s if (getRosBoolean(node_, "publish.hnr.pvt")) { hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); } + + imu_pub_ = node_->create_publisher("~/imu_meas", 1); + imu_att_pub_ = node_->create_publisher("~/imu_att", 1); + imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); + imu_temp_pub_ = node_->create_publisher("~/imu_temp", 1); + esf_ins_ros_pub_ = node_->create_publisher("~/kinematics", 1); + fix_hp_pub_ = node_->create_publisher("~/fix", 1); + nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); + + // Perform any message metadata value setting we can do only once, including default values + // This only improves performance a little, but removes duplcate code + imu_.header.frame_id = frame_id_; + imu_.orientation_covariance[0] = -1.0; + imu_.orientation_covariance[4] = -1.0; + imu_.orientation_covariance[8] = -1.0; + imu_.linear_acceleration_covariance[0] = -1.0; + imu_.linear_acceleration_covariance[4] = -1.0; + imu_.linear_acceleration_covariance[8] = -1.0; + imu_.angular_velocity_covariance[0] = -1.0; + imu_.angular_velocity_covariance[4] = -1.0; + imu_.angular_velocity_covariance[8] = -1.0; + + imu_raw_ = imu_; // Initialize using the same values as bove + imu_att_ = imu_; // Initialize using the same values as above + + imu_temp_.header.frame_id = frame_id_; + imu_temp_.variance = 0.0; // signifies unknown variance + imu_temp_.temperature = 0.0; // signifies missing data + + esf_ins_ros_.header.frame_id = frame_id_; + esf_ins_ros_.linear_acceleration_covariance[0] = -1.0; // signifies missing data + esf_ins_ros_.angular_velocity_covariance[0] = -1.0; // signifies missing data + + fix_hp_.header.frame_id = frame_id_; + fix_hp_.position_covariance[0] = -1.0; + fix_hp_.position_covariance[4] = -1.0; + fix_hp_.position_covariance[8] = -1.0; + fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + + nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + nav_diag_.name = "NavigationDiagnostics"; + nav_diag_.message = "EsfSTATUS"; + nav_diag_.hardware_id = product_model_; } + +void AdrUdrProduct::subscribe(std::shared_ptr gps) { + + // Subscribe to the Position-Velocity-Time solution messages. + // These provide important metadata + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavPVT, this, std::placeholders::_1), 1); + + // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); + + // Sensor Fusion status for diagnostics + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); + + // Subscribe to ADR/UDR Navigation Attitude messages + if (getRosBoolean(node_, "publish.nav.att")) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavATT, this, std::placeholders::_1), 1); + } + + // Subscribe to ADR/UDR Inertial Navigation System kinematics messages + if (getRosBoolean(node_, "publish.esf.ins")) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfIns, this, std::placeholders::_1), 1); + } + + // Subscribe to ADR/UDR Post-Processed IMU messages + if (getRosBoolean(node_, "publish.esf.meas")) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); + } + + // Subscribe to ADR/UDR Raw IMU messages + if (getRosBoolean(node_, "publish.esf.raw")) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); + } + + // Subscribe to ESF Status messages + if (getRosBoolean(node_, "publish.esf.status")) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); + } + + // Subscribe to High-Navigation rate PVT messages + if (getRosBoolean(node_, "publish.hnr.pvt")) { + gps->subscribe([this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, + 1); + } +} + + void AdrUdrProduct::getRosParams() { use_adr_ = getRosBoolean(node_, "use_adr"); // Check the nav rate @@ -84,177 +184,320 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { return true; } -void AdrUdrProduct::subscribe(std::shared_ptr gps) { - // Subscribe to NAV ATT messages +void AdrUdrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { + ublox_msgs::msg::NavATT temp_att = last_nav_att_; + if (temp_att.i_tow == m.i_tow) { + esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; + esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; + imu_att_.header.stamp = node_->now(); + } if (getRosBoolean(node_, "publish.nav.att")) { - gps->subscribe([this](const ublox_msgs::msg::NavATT &m) { nav_att_pub_->publish(m); }, - 1); + nav_att_pub_->publish(m); } - // Subscribe to ESF INS messages + constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + + // Transform U-Blox Euler angles to a Quaternion and populate covariances + const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); + tf2::Quaternion orientation; + orientation.setRPY(roll, pitch, heading); + + imu_att_.orientation.x = orientation[0]; + imu_att_.orientation.y = orientation[1]; + imu_att_.orientation.z = orientation[2]; + imu_att_.orientation.w = orientation[3]; + + imu_att_.orientation_covariance[0] = std::pow(static_cast(m.acc_roll) * kNavAttScaleAndRadianConversion, 2.0); + imu_att_.orientation_covariance[4] = std::pow(static_cast(m.acc_pitch) * kNavAttScaleAndRadianConversion, 2.0); + imu_att_.orientation_covariance[8] = std::pow(static_cast(m.acc_heading) * kNavAttScaleAndRadianConversion, 2.0); + + imu_att_pub_->publish(imu_att_); + last_nav_att_ = m; +} + +void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { + esf_ins_ros_.header.stamp = node_->now(); + if (getRosBoolean(node_, "publish.esf.ins")) { - gps->subscribe([this](const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); }, - 1); + esf_ins_pub_->publish(m); } - // Subscribe to ESF Meas messages - 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); - } + // To avoid long mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID + ublox_msgs::msg::NavATT temp_att = last_nav_att_; + // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT + if (temp_att.i_tow == m.i_tow) { + constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - // Subscribe to ESF Raw messages - if (getRosBoolean(node_, "publish.esf.raw")) { - gps->subscribe([this](const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); }, - 1); + const double roll = M_PI_2 - (static_cast(temp_att.roll) * kNavAttScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(temp_att.pitch) * kNavAttScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(temp_att.heading) * kNavAttScaleAndRadianConversion); + tf2::Quaternion orientation; + orientation.setRPY(roll, pitch, heading); - // also publish sensor_msgs::Imu for the raw data - gps->subscribe(std::bind( - &AdrUdrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); - } + esf_ins_ros_.orientation.x = orientation[0]; + esf_ins_ros_.orientation.y = orientation[1]; + esf_ins_ros_.orientation.z = orientation[2]; + esf_ins_ros_.orientation.w = orientation[3]; - // Subscribe to ESF Status messages - if (getRosBoolean(node_, "publish.esf.status")) { - gps->subscribe([this](const ublox_msgs::msg::EsfSTATUS &m) { esf_status_pub_->publish(m); }, - 1); + esf_ins_ros_.orientation_covariance[0] = std::pow(temp_att.acc_roll * kNavAttScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[4] = std::pow(temp_att.acc_pitch * kNavAttScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[8] = std::pow(temp_att.acc_heading * kNavAttScaleAndRadianConversion, 2); + } else { // No data available for this data frame + esf_ins_ros_.orientation_covariance[0] = -1.0; + esf_ins_ros_.orientation_covariance[4] = -1.0; + esf_ins_ros_.orientation_covariance[8] = -1.0; } - // Subscribe to HNR PVT messages - if (getRosBoolean(node_, "publish.hnr.pvt")) { - gps->subscribe([this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, - 1); - } + constexpr double kMilliGramsToNewtons{1e-6}; + constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; + + esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.x = static_cast(m.x_accel) * kMilliGramsToNewtons; + esf_ins_ros_.angular_velocity.y = static_cast(m.y_accel) * kMilliGramsToNewtons; + esf_ins_ros_.angular_velocity.z = static_cast(m.z_accel) * kMilliGramsToNewtons; + + esf_ins_ros_pub_->publish(esf_ins_ros_); } -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_; - imu_.orientation_covariance[0] = -1; - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - const std::vector imu_data = m.data; - for (const std::uint32_t 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 - - //grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer - const std::int32_t data_value = extract_int24(datapoint); - - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - break; - imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); - break; // Reserved for future use - // The following are not currently used - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; - - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } +const char* wt_status_str(std::uint8_t bitfield) { + std::uint8_t wt_status = bitfield & 0xb00000011; + if (wt_status == 2) return "calibrated"; + if (wt_status == 1) return "initializing"; + if (wt_status == 0) return "off"; + return "deserialization error"; +} + +const char* imu_alg_str(std::uint8_t bitfield) { + std::uint8_t alg_status = bitfield & 0xb00011100; + if (alg_status == 2) return "calibrated"; + if (alg_status == 1) return "initializing"; + if (alg_status == 0) return "off"; + return "deserialization error"; +} - // create time ref message and put in the data - t_ref_.header.seq = m.risingEdgeCount; - t_ref_.header.stamp = node_->now(); - t_ref_.header.frame_id = frame_id_; +const char* ins_init_str(std::uint8_t bitfield) { + std::uint8_t ins_status = bitfield & 0xb01100000; + if (ins_status == 2) return "calibrated"; + if (ins_status == 1) return "initializing"; + if (ins_status == 0) return "off"; + return "deserialization error"; +} - // Consider removing the below - t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); +const char* imu_init_str(std::uint8_t bitfield) { + std::uint8_t imu_status = bitfield & 0xb00000011; + if (imu_status == 2) return "calibrated"; + if (imu_status == 1) return "initializing"; + if (imu_status == 0) return "off"; + return "deserialization error"; +} - time_ref_pub_->publish(t_ref_); - imu_pub_->publish(imu_); +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + const rclcpp::Time callback_time{node_->now()}; + // This is time-critical data, so if something is expecting it, republish early + if (getRosBoolean(node_, "publish.esf.meas")) { + esf_meas_pub_->publish(m); + } - // Consider removing the below - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); + for (const std::uint32_t datapoint : m.data) { + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer + const std::int32_t data_value = extract_int24(datapoint); + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius; + break; // Reserved for future use + // The following are not currently used + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); } + imu_.header.stamp = callback_time; + imu_pub_->publish(imu_); + imu_temp_.header.stamp = callback_time; + imu_temp_pub_->publish(imu_temp_); } +} +// +// Parse the sensor fusion status information +// +void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { + diagnostic_msgs::msg::KeyValue wt_status; + wt_status.key = "wheel_tick_status"; + wt_status.value = wt_status_str(m.reserved1[0]); + diagnostic_msgs::msg::KeyValue imu_alg; + imu_alg.key = "IMU_alignment_status"; + imu_alg.value = imu_alg_str(m.reserved1[0]); + diagnostic_msgs::msg::KeyValue ins_ini; + ins_ini.key = "INS_init_status"; + ins_ini.value = ins_init_str(m.reserved1[0]); + diagnostic_msgs::msg::KeyValue imu_ini; + imu_ini.key = "IMU_init_status"; + imu_ini.value = imu_init_str(m.reserved1[1]); + diagnostic_msgs::msg::KeyValue fusion_mode; + fusion_mode.key = "fusion_mode"; + fusion_mode.value = m.fusion_mode == 3 ? "disabled_fault" : (m.fusion_mode == 2 ? "suspended" : (m.fusion_mode == 1 ? "online" : "initializing")); + diagnostic_msgs::msg::KeyValue num_sens; + num_sens.key = "num_sensors"; + num_sens.value = std::to_string(m.num_sens); + + nav_diag_.values.push_back(imu_alg); + nav_diag_.values.push_back(imu_ini); + nav_diag_.values.push_back(wt_status); + nav_diag_.values.push_back(ins_ini); + nav_diag_.values.push_back(fusion_mode); + nav_diag_.values.push_back(num_sens); + + nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + nav_diag_pub_->publish(nav_diag_); } + +// +// Decode the Raw IMU measurement output +// void AdrUdrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { + const rclcpp::Time callback_time{node_->now()}; + // This is time-critical data, so if something is expecting it, republish early if (getRosBoolean(node_, "publish.esf.raw")) { - imu_raw_.header.stamp = node_->now(); - imu_raw_.header.frame_id = frame_id_; - imu_raw_.orientation_covariance[0] = -1; - imu_raw_.linear_acceleration_covariance[0] = -1; - imu_raw_.angular_velocity_covariance[0] = -1; - - const std::vector imu_data_blocks = m.blocks; - for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : imu_data_blocks) { - const std::uint32_t datapoint = imu_data_entry.data; - - //grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer, and cast to double - const std::int32_t data_value = extract_int24(datapoint); - - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - break; // Reserved for future use - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; // Reserved for future use - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } + esf_raw_pub_->publish(m); + } + + for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : m.blocks) { + const std::uint32_t datapoint = imu_data_entry.data; + // Grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer, and cast to double + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; + + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius; break; + // The following are not currently used; they relate to sensor fusion with wheel encoder data + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); } + imu_raw_.header.stamp = callback_time; imu_raw_pub_->publish(imu_raw_); + imu_temp_.header.stamp = callback_time; + imu_temp_pub_->publish(imu_temp_); + } +} + +// +// Decode the High-Precision Geodetic Position message (HPPOSLLH) into NavSatFix +// +void AdrUdrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { + // Do not publish invalid HPPOSLLH data + if (m.flags != 0) { + return; + } + + if (getRosBoolean(node_, "publish.nav.hpposllh")) { + nav_hpposllh_pub_->publish(m); + } + + fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device + + if (last_nav_pvt_.fix_type >= ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { + fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + } else { + fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + } + + // Calculate the high-precision lat, lon, alt values + fix_hp_.latitude = 1e-7 * (static_cast(m.lat) + (static_cast(m.lat_hp) * 1e-2)); + fix_hp_.longitude = 1e-7 * (static_cast(m.lon) + (static_cast(m.lon_hp) * 1e-2)); + fix_hp_.altitude = 1e-3 * (static_cast(m.height) + (static_cast(m.height_hp) * 1e-1)); + + // Populate the covariance data + const double var_h = std::pow(m.h_acc / 1000.0, 2); + const double var_v = std::pow(m.v_acc / 1000.0, 2); + fix_hp_.position_covariance[0] = var_h; + fix_hp_.position_covariance[4] = var_h; + fix_hp_.position_covariance[8] = var_v; + fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + fix_hp_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + fix_hp_pub_->publish(fix_hp_); +} + +// +// Use NavPVT for some message fusion diagnostics data +// +void AdrUdrProduct::callbackNavPVT(const ublox_msgs::msg::NavPVT& m) { + if (getRosBoolean(node_, "publish.nav.pvt")) { + nav_pvt_pub_->publish(m); + } + last_nav_pvt_ = m; + + // update the iTow 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 + last_itow_time_.first = m.i_tow; + if (m.nano < 0) { + last_itow_time_.second.sec = ublox_node::toUtcSeconds(m) - 1; + last_itow_time_.second.nanosec = static_cast(m.nano + 1e9); + } + else { + last_itow_time_.second.sec = ublox_node::toUtcSeconds(m); + last_itow_time_.second.nanosec = static_cast(m.nano); + } } } ->>>>>>> 0cb2c06 (Add full product support for ZED-F9R) } // namespace ublox_node diff --git a/ublox_gps/src/hpg_dr_product.cpp b/ublox_gps/src/hpg_dr_product.cpp index addf7bbd..0d6d05d1 100644 --- a/ublox_gps/src/hpg_dr_product.cpp +++ b/ublox_gps/src/hpg_dr_product.cpp @@ -20,13 +20,9 @@ namespace ublox_node { // Extract U-Blox 24-bit signed integers from a 32-bit data blob // and cast into int32_t // -static inline std::int32_t extract_int24(std::uint32_t data) { - const std::uint8_t* b = reinterpret_cast(&data); - return (std::int32_t)( - (((std::uint32_t)b[2] << 24) & 0xFF000000) | - (((std::uint32_t)b[1] << 16) & 0x00FF0000) | - (((std::uint32_t)b[0] << 8) & 0x0000FF00) - ) / 256; +static inline std::int32_t extract_int24(std::uint32_t bitfield) { + std::int32_t temp = static_cast(bitfield << 8); + return temp >> 8; } // @@ -57,29 +53,35 @@ HpgDrProduct::HpgDrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::str imu_meas_pub_ = node_->create_publisher("~/imu_meas", 1); imu_att_pub_ = node_->create_publisher("~/imu_att", 1); - esf_ins_ros_pub_ = node_->create_publisher("~/veh_kinematics", 1); imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); + imu_temp_pub_ = node_->create_publisher("~/imu_temp", 1); + esf_ins_ros_pub_ = node_->create_publisher("~/veh_kinematics", 1); fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); // Perform any message metadata value setting we can do only once, including default values // This only improves performance a little, but removes duplcate code imu_meas_.header.frame_id = frame_id_; + imu_meas_.orientation_covariance[0] = -1; + imu_meas_.linear_acceleration_covariance[0] = -1; + imu_meas_.angular_velocity_covariance[0] = -1; - imu_raw_.header.frame_id = frame_id_; - imu_raw_.orientation_covariance[0] = -1; - imu_raw_.linear_acceleration_covariance[0] = -1; - imu_raw_.angular_velocity_covariance[0] = -1; + imu_raw_ = imu_meas_; // Initialize using the same values as bove + imu_att_ = imu_meas_; // Initialize using the same values as above - imu_att_.header.frame_id = frame_id_; - imu_att_.linear_acceleration_covariance[0] = -1; // signifies missing data - imu_att_.angular_velocity_covariance[0] = -1; // signifies missing data + imu_temp_.header.frame_id = frame_id_; + imu_temp_.variance = 0.0; // signifies unknown variance + imu_temp_.temperature = 0.0; // signifies missing data esf_ins_ros_.header.frame_id = frame_id_; esf_ins_ros_.linear_acceleration_covariance[0] = -1; // signifies missing data esf_ins_ros_.angular_velocity_covariance[0] = -1; // signifies missing data fix_hp_.header.frame_id = frame_id_; + fix_hp_.position_covariance[0] = -1.0; + fix_hp_.position_covariance[4] = -1.0; + fix_hp_.position_covariance[8] = -1.0; + fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } void HpgDrProduct::subscribe(std::shared_ptr gps) { @@ -116,7 +118,7 @@ void HpgDrProduct::getRosParams() { // Check the nav rate float nav_rate_hz = 1000.0 / (meas_rate_ * nav_rate_); if (nav_rate_hz != 1) { - RCLCPP_WARN(node_->get_logger(), "ADR/UDR Nav Rate recommended to be 1 Hz"); + RCLCPP_WARN(node_->get_logger(), "Nav Rate recommended to be 1 Hz"); } } @@ -141,7 +143,7 @@ void HpgDrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - // Transform U-Blox Euler angles to Quaternion and populate covariances + // Transform U-Blox Euler angles to a Quaternion and populate covariances const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); @@ -153,9 +155,9 @@ void HpgDrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { imu_att_.orientation.z = orientation[2]; imu_att_.orientation.w = orientation[3]; - imu_att_.orientation_covariance[0] = std::pow(m.acc_roll * kNavAttScaleAndRadianConversion, 2); - imu_att_.orientation_covariance[4] = std::pow(m.acc_pitch * kNavAttScaleAndRadianConversion, 2); - imu_att_.orientation_covariance[8] = std::pow(m.acc_heading * kNavAttScaleAndRadianConversion, 2); + imu_att_.orientation_covariance[0] = std::pow(static_cast(m.acc_roll) * kNavAttScaleAndRadianConversion, 2.0); + imu_att_.orientation_covariance[4] = std::pow(static_cast(m.acc_pitch) * kNavAttScaleAndRadianConversion, 2.0); + imu_att_.orientation_covariance[8] = std::pow(static_cast(m.acc_heading) * kNavAttScaleAndRadianConversion, 2.0); imu_att_pub_->publish(imu_att_); last_nav_att_ = m; @@ -207,7 +209,6 @@ void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_ros_pub_->publish(esf_ins_ros_); } - const char* wt_status_str(std::uint8_t bitfield) { std::uint8_t wt_status = bitfield & 0xb00000011; if (wt_status == 2) return "calibrated"; @@ -240,7 +241,76 @@ const char* imu_init_str(std::uint8_t bitfield) { return "deserialization error"; } +void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + if (getRosBoolean(node_, "publish.esf.meas")) { + imu_.header.stamp = node_->now(); + imu_.header.frame_id = frame_id_; + imu_.orientation_covariance[0] = -1; + imu_.linear_acceleration_covariance[0] = -1; + imu_.angular_velocity_covariance[0] = -1; + + const std::vector imu_data = m.data; + for (const std::uint32_t datapoint : imu_data) { + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer + const std::int32_t data_value = extract_int24(datapoint); + + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + break; + imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); + break; // Reserved for future use + // The following are not currently used + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; + + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); + } + + // create time ref message and put in the data + t_ref_.seq = m.risingEdgeCount; + t_ref_.header.stamp = node_->now(); + t_ref_.header.frame_id = frame_id_; + + // Consider removing the below + t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); + + time_ref_pub_->publish(t_ref_); + imu_pub_->publish(imu_); + + // Consider removing the below + //std::ostringstream src; + //src << "TIM" << int(m.ch); + //t_ref_.source = src.str(); + } + } +} void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { diagnostic_msgs::msg::KeyValue wt_status; wt_status.key = "wheel_tick_status"; @@ -261,30 +331,14 @@ void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { num_sens.key = "num_sensors"; num_sens.value = std::to_string(m.num_sens); - diagnostic_msgs::msg::DiagnosticStatus nav_diag; - nav_diag.name = "NavigationDiagnostics"; - nav_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - nav_diag.hardware_id = "ublox ZED-F9"; - - nav_diag.values.push_back(imu_alg); - nav_diag.values.push_back(imu_ini); - nav_diag.values.push_back(wt_status); - nav_diag.values.push_back(ins_ini); - nav_diag.values.push_back(fusion_mode); - nav_diag.values.push_back(num_sens); - nav_diag_pub_->publish(nav_diag); -} - -// -// Decode the Processed IMU measurement output -// -void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { - imu_meas_.header.stamp = node_->now(); - - if (getRosBoolean(node_, "publish.esf.meas")) { - esf_meas_pub_->publish(m); + // Consider removing the below + //std::ostringstream src; + //src << "TIM" << int(m.ch); + //t_ref_.source = src.str(); + } } + imu_meas_.header.stamp = node_->now(); imu_meas_.orientation_covariance[0] = -1; imu_meas_.linear_acceleration_covariance[0] = -1; imu_meas_.angular_velocity_covariance[0] = -1; @@ -300,35 +354,29 @@ void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: imu_meas_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_meas_.linear_acceleration.x = static_cast(data_value) * kConvertMPerSecSq; break; + imu_meas_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: imu_meas_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_meas_.linear_acceleration.y = static_cast(data_value) * kConvertMPerSecSq; break; + imu_meas_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: imu_meas_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_meas_.linear_acceleration.z = static_cast(data_value) * kConvertMPerSecSq; break; + imu_meas_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - break; // Do nothing, just catch + imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); + + imu_temp_.header.stamp = callback_time; + imu_temp_pub_->publish(imu_temp_); + break; + // The below are not currently used case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_FL %d\n", data_value); - break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_FR %d\n", data_value); - break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_RL %d\n", data_value); - break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_RR %d\n", data_value); - break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_SINGLE %d\n", data_value); - break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - RCLCPP_INFO(node_->get_logger(), "WHEEL_TICK_SPEED %d\n", data_value); - break; // Do nothing, just catch + break; default: RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); } @@ -344,12 +392,16 @@ void HpgDrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); } imu_raw_.header.stamp = node_->now(); + imu_raw_.header.frame_id = frame_id_; + imu_raw_.orientation_covariance[0] = -1; + imu_raw_.linear_acceleration_covariance[0] = -1; + imu_raw_.angular_velocity_covariance[0] = -1; const std::vector imu_data_blocks = m.blocks; for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : imu_data_blocks) { const std::uint32_t datapoint = imu_data_entry.data; - //grab the last six bits of data as the data type description field + // Grab the last six bits of data as the data type description field const std::uint8_t data_type = datapoint >> 24; // Interpret the first 24 bits as a signed integer, and cast to double const std::int32_t data_value = extract_int24(datapoint); @@ -358,15 +410,15 @@ void HpgDrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMPerSecSq; break; + imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMPerSecSq; break; + imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMPerSecSq; break; + imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: break; // Do nothing, just catch case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 9bc01032..8aff295a 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -231,7 +230,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision GNSS Rovers"); // High-Precision GNSS Sensor Fusion products with Dead Reckoning capability } else if (product_category == "HPS" && (product_model == "ZED-F9R" || product_model == "ZED-F9K")) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, rtcms_, this)); + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, product_model, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision Dead Reckoning products."); // Any other High-Precision GNSS product, without sensor fusion } else if (product_category == "HPG" || product_category == "HPS") { @@ -244,7 +243,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, // Automotive Dead Reckoning or Untethered Dead Reckoning products not detected above } 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(nav_rate_, meas_rate_, frame_id_, updater_, product_model, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for Automotive/Untethered Dead Reckoning products"); // Frequency & other Time Synchronization products } else if (product_category == "FTS") { diff --git a/ublox_mcu-config b/ublox_mcu-config deleted file mode 100644 index 20d34b70..00000000 --- a/ublox_mcu-config +++ /dev/null @@ -1,8 +0,0 @@ -# Wheel speed RR -0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x27 0x10 0x91 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x39 0x01 0xe7 0x2d - -# Wheel speed RL -0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x37 0x10 0x92 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x29 0x01 0xe8 0xd7 - -# Direction (Drive gear engaged or not) -0x43 0xa2 0x11 0x13 0x03 0xbc 0x03 0x00 0x00 0x08 0x64 0x2f 0x01 0x00 0x00 0x01 0x00 0x00 0x00 0xe8 0x03 0x5f 0x01 0xce 0xb8 \ No newline at end of file From 32f498091d57b0da4afd43c3c30e50e24bf40839 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 18:07:33 -0800 Subject: [PATCH 04/13] Clean up --- .../include/ublox_gps/hpg_dr_product.hpp | 128 ----- ublox_gps/src/hpg_dr_product.cpp | 511 ------------------ 2 files changed, 639 deletions(-) delete mode 100644 ublox_gps/include/ublox_gps/hpg_dr_product.hpp delete mode 100644 ublox_gps/src/hpg_dr_product.cpp diff --git a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp b/ublox_gps/include/ublox_gps/hpg_dr_product.hpp deleted file mode 100644 index 6f69c2ac..00000000 --- a/ublox_gps/include/ublox_gps/hpg_dr_product.hpp +++ /dev/null @@ -1,128 +0,0 @@ -#ifndef UBLOX_GPS_HPG_DR_PRODUCT_HPP -#define UBLOX_GPS_HPG_DR_PRODUCT_HPP - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace ublox_node { - -/** - * @brief High-Precision GNSS Dead Reckoning product interface for firmware >= 8 - */ -class HpgDrProduct final : public virtual ComponentInterface { - public: - explicit HpgDrProduct(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 High-Precision GNSS DR messages. - */ - void subscribe(std::shared_ptr gps) override; - - - /** - * @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; - - void initializeRosDiagnostics() override { - // RCLCPP_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s", - // "unimplemented. See AdrUdrProduct class in node.hpp & node.cpp."); - } - - private: - - // Scaling factors for floating point representations of data - //static constexpr float kConvertRadPerSec = std::pow(2, -12) * M_PI / 180.0F; - //static constexpr float kConvertMPerSecSq = std::pow(2, -10); - //static constexpr float kConvertDegCelsius{0.01F}; - - // Callbacks relevant to ROS2 message output triggered on U-Blox message events - void callbackEsfIns(const ublox_msgs::msg::EsfINS &m); - void callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m); - void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); - void callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m); - void callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH &m); - void callbackNavATT(const ublox_msgs::msg::NavATT &m); - void callbackNavPvt(const ublox_msgs::msg::NavPVT &m); - - // Reused containers for ROS2 messages - sensor_msgs::msg::Imu imu_meas_; - sensor_msgs::msg::Imu imu_raw_; - sensor_msgs::msg::Imu imu_att_; - sensor_msgs::msg::Imu esf_ins_ros_; - sensor_msgs::msg::Temperature imu_temp_; - - sensor_msgs::msg::NavSatFix fix_hp_; - diagnostic_msgs::msg::DiagnosticStatus nav_diag_; - - //! Last global position (used for diagnostic updater) - ublox_msgs::msg::NavHPPOSLLH last_hp_pos_; - - // ROS2 publishers for ROS2 messages - rclcpp::Publisher::SharedPtr imu_meas_pub_; - rclcpp::Publisher::SharedPtr imu_raw_pub_; - rclcpp::Publisher::SharedPtr imu_att_pub_; - rclcpp::Publisher::SharedPtr imu_temp_pub_; - rclcpp::Publisher::SharedPtr esf_ins_ros_pub_; - rclcpp::Publisher::SharedPtr fix_hp_pub_; - rclcpp::Publisher::SharedPtr nav_diag_pub_; - - // ROS2 republishers for U-Blox messages - rclcpp::Publisher::SharedPtr nav_att_pub_; - rclcpp::Publisher::SharedPtr nav_hpposllh_pub_; - rclcpp::Publisher::SharedPtr nav_pvt_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_; - - // Relevant configuration state parameters - bool use_adr_; - uint16_t nav_rate_; - uint16_t meas_rate_; - - std::string frame_id_; - - std::shared_ptr updater_; - std::vector rtcms_; - rclcpp::Node* node_; - - // Local cache of U-Blox messages to be fused within a navigation epoch (data frame) - ublox_msgs::msg::NavATT last_nav_att_; - ublox_msgs::msg::NavPVT last_nav_pvt_; - std::pair last_itow_time_; - -}; - -} // namespace ublox_node - -#endif // UBLOX_GPS_HPG_DR_PRODUCT_HPP diff --git a/ublox_gps/src/hpg_dr_product.cpp b/ublox_gps/src/hpg_dr_product.cpp deleted file mode 100644 index 0d6d05d1..00000000 --- a/ublox_gps/src/hpg_dr_product.cpp +++ /dev/null @@ -1,511 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace ublox_node { - -// -// Extract U-Blox 24-bit signed integers from a 32-bit data blob -// and cast into int32_t -// -static inline std::int32_t extract_int24(std::uint32_t bitfield) { - std::int32_t temp = static_cast(bitfield << 8); - return temp >> 8; -} - -// -// U-Blox High Precision GNSS product with Dead Reckoning -// These appear to only have firmware version >= 8 -// -HpgDrProduct::HpgDrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, std::vector rtcms, rclcpp::Node* node) - : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), rtcms_(rtcms), node_(node) -{ - if (getRosBoolean(node_, "publish.esf.meas")) { - 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.nav.pvt")) { - nav_pvt_pub_ = node_->create_publisher("navpvt", 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); - } - - imu_meas_pub_ = node_->create_publisher("~/imu_meas", 1); - imu_att_pub_ = node_->create_publisher("~/imu_att", 1); - imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); - imu_temp_pub_ = node_->create_publisher("~/imu_temp", 1); - esf_ins_ros_pub_ = node_->create_publisher("~/veh_kinematics", 1); - fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); - nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); - - // Perform any message metadata value setting we can do only once, including default values - // This only improves performance a little, but removes duplcate code - imu_meas_.header.frame_id = frame_id_; - imu_meas_.orientation_covariance[0] = -1; - imu_meas_.linear_acceleration_covariance[0] = -1; - imu_meas_.angular_velocity_covariance[0] = -1; - - imu_raw_ = imu_meas_; // Initialize using the same values as bove - imu_att_ = imu_meas_; // Initialize using the same values as above - - imu_temp_.header.frame_id = frame_id_; - imu_temp_.variance = 0.0; // signifies unknown variance - imu_temp_.temperature = 0.0; // signifies missing data - - esf_ins_ros_.header.frame_id = frame_id_; - esf_ins_ros_.linear_acceleration_covariance[0] = -1; // signifies missing data - esf_ins_ros_.angular_velocity_covariance[0] = -1; // signifies missing data - - fix_hp_.header.frame_id = frame_id_; - fix_hp_.position_covariance[0] = -1.0; - fix_hp_.position_covariance[4] = -1.0; - fix_hp_.position_covariance[8] = -1.0; - fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; -} - -void HpgDrProduct::subscribe(std::shared_ptr gps) { - // Subscribe to ADR/UDR Navigation Attitude messages - gps->subscribe(std::bind( - &HpgDrProduct::callbackNavATT, this, std::placeholders::_1), 1); - - gps->subscribe(std::bind( - &HpgDrProduct::callbackNavPvt, this, std::placeholders::_1), 1); - - // ESF status for diagnostics - gps->subscribe(std::bind( - &HpgDrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); - - // Subscribe to High-Precision Geodetic Position messages - gps->subscribe(std::bind( - &HpgDrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); - - // Subscribe to ADR/UDR Post-Processed IMU messages - gps->subscribe(std::bind( - &HpgDrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); - - // Subscribe to ADR/UDR Inertial Navigation System kinematics messages - gps->subscribe(std::bind( - &HpgDrProduct::callbackEsfIns, this, std::placeholders::_1), 1); - - // Subscribe to ADR/UDR Raw IMU messages - gps->subscribe(std::bind( - &HpgDrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); -} - -void HpgDrProduct::getRosParams() { - use_adr_ = getRosBoolean(node_, "use_adr"); - // Check the 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"); - } -} - -bool HpgDrProduct::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 HpgDrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { - ublox_msgs::msg::NavATT temp_att = last_nav_att_; - if (temp_att.i_tow == m.i_tow) { - esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; - esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; - imu_att_.header.stamp = node_->now(); - } - if (getRosBoolean(node_, "publish.nav.att")) { - nav_att_pub_->publish(m); - } - - constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - - // Transform U-Blox Euler angles to a Quaternion and populate covariances - const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); - const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); - const double heading = M_PI_2 - (static_cast(m.heading) * kNavAttScaleAndRadianConversion); - tf2::Quaternion orientation; - orientation.setRPY(roll, pitch, heading); - - imu_att_.orientation.x = orientation[0]; - imu_att_.orientation.y = orientation[1]; - imu_att_.orientation.z = orientation[2]; - imu_att_.orientation.w = orientation[3]; - - imu_att_.orientation_covariance[0] = std::pow(static_cast(m.acc_roll) * kNavAttScaleAndRadianConversion, 2.0); - imu_att_.orientation_covariance[4] = std::pow(static_cast(m.acc_pitch) * kNavAttScaleAndRadianConversion, 2.0); - imu_att_.orientation_covariance[8] = std::pow(static_cast(m.acc_heading) * kNavAttScaleAndRadianConversion, 2.0); - - imu_att_pub_->publish(imu_att_); - last_nav_att_ = m; -} - -void HpgDrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { - esf_ins_ros_.header.stamp = node_->now(); - - if (getRosBoolean(node_, "publish.esf.ins")) { - esf_ins_pub_->publish(m); - } - - // To avoid long mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID - ublox_msgs::msg::NavATT temp_att = last_nav_att_; - // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT - if (temp_att.i_tow == m.i_tow) { - constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - - const double roll = M_PI_2 - (static_cast(temp_att.roll) * kNavAttScaleAndRadianConversion); - const double pitch = M_PI_2 - (static_cast(temp_att.pitch) * kNavAttScaleAndRadianConversion); - const double heading = M_PI_2 - (static_cast(temp_att.heading) * kNavAttScaleAndRadianConversion); - tf2::Quaternion orientation; - orientation.setRPY(roll, pitch, heading); - - esf_ins_ros_.orientation.x = orientation[0]; - esf_ins_ros_.orientation.y = orientation[1]; - esf_ins_ros_.orientation.z = orientation[2]; - esf_ins_ros_.orientation.w = orientation[3]; - - esf_ins_ros_.orientation_covariance[0] = std::pow(temp_att.acc_roll * kNavAttScaleAndRadianConversion, 2); - esf_ins_ros_.orientation_covariance[4] = std::pow(temp_att.acc_pitch * kNavAttScaleAndRadianConversion, 2); - esf_ins_ros_.orientation_covariance[8] = std::pow(temp_att.acc_heading * kNavAttScaleAndRadianConversion, 2); - } else { // No data available for this data frame - esf_ins_ros_.orientation_covariance[0] = -1; - esf_ins_ros_.orientation_covariance[4] = -1; - esf_ins_ros_.orientation_covariance[8] = -1; - } - - constexpr double kMilliGramsToNewtons{1e-6}; - constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; - - esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.x = static_cast(m.x_accel) * kMilliGramsToNewtons; - esf_ins_ros_.angular_velocity.y = static_cast(m.y_accel) * kMilliGramsToNewtons; - esf_ins_ros_.angular_velocity.z = static_cast(m.z_accel) * kMilliGramsToNewtons; - - esf_ins_ros_pub_->publish(esf_ins_ros_); -} - -const char* wt_status_str(std::uint8_t bitfield) { - std::uint8_t wt_status = bitfield & 0xb00000011; - if (wt_status == 2) return "calibrated"; - if (wt_status == 1) return "initializing"; - if (wt_status == 0) return "off"; - return "deserialization error"; -} - -const char* imu_alg_str(std::uint8_t bitfield) { - std::uint8_t alg_status = bitfield & 0xb00011100; - if (alg_status == 2) return "calibrated"; - if (alg_status == 1) return "initializing"; - if (alg_status == 0) return "off"; - return "deserialization error"; -} - -const char* ins_init_str(std::uint8_t bitfield) { - std::uint8_t ins_status = bitfield & 0xb01100000; - if (ins_status == 2) return "calibrated"; - if (ins_status == 1) return "initializing"; - if (ins_status == 0) return "off"; - return "deserialization error"; -} - -const char* imu_init_str(std::uint8_t bitfield) { - std::uint8_t imu_status = bitfield & 0xb00000011; - if (imu_status == 2) return "calibrated"; - if (imu_status == 1) return "initializing"; - if (imu_status == 0) return "off"; - return "deserialization error"; -} - -void HpgDrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { - if (getRosBoolean(node_, "publish.esf.meas")) { - imu_.header.stamp = node_->now(); - imu_.header.frame_id = frame_id_; - imu_.orientation_covariance[0] = -1; - imu_.linear_acceleration_covariance[0] = -1; - imu_.angular_velocity_covariance[0] = -1; - - const std::vector imu_data = m.data; - for (const std::uint32_t datapoint : imu_data) { - //grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer - const std::int32_t data_value = extract_int24(datapoint); - - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - break; - imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); - break; // Reserved for future use - // The following are not currently used - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; - - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } - - // create time ref message and put in the data - t_ref_.seq = m.risingEdgeCount; - t_ref_.header.stamp = node_->now(); - t_ref_.header.frame_id = frame_id_; - - // Consider removing the below - t_ref_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - time_ref_pub_->publish(t_ref_); - imu_pub_->publish(imu_); - - // Consider removing the below - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); - } - } - -} -void HpgDrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { - diagnostic_msgs::msg::KeyValue wt_status; - wt_status.key = "wheel_tick_status"; - wt_status.value = wt_status_str(m.reserved1[0]); - diagnostic_msgs::msg::KeyValue imu_alg; - imu_alg.key = "IMU_alignment_status"; - imu_alg.value = imu_alg_str(m.reserved1[0]); - diagnostic_msgs::msg::KeyValue ins_ini; - ins_ini.key = "INS_init_status"; - ins_ini.value = ins_init_str(m.reserved1[0]); - diagnostic_msgs::msg::KeyValue imu_ini; - imu_ini.key = "IMU_init_status"; - imu_ini.value = imu_init_str(m.reserved1[1]); - diagnostic_msgs::msg::KeyValue fusion_mode; - fusion_mode.key = "fusion_mode"; - fusion_mode.value = m.fusion_mode == 3 ? "disabled_fault" : (m.fusion_mode == 2 ? "suspended" : (m.fusion_mode == 1 ? "online" : "initializing")); - diagnostic_msgs::msg::KeyValue num_sens; - num_sens.key = "num_sensors"; - num_sens.value = std::to_string(m.num_sens); - - // Consider removing the below - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); - } - } - - imu_meas_.header.stamp = node_->now(); - imu_meas_.orientation_covariance[0] = -1; - imu_meas_.linear_acceleration_covariance[0] = -1; - imu_meas_.angular_velocity_covariance[0] = -1; - - const std::vector imu_data = m.data; - for (const std::uint32_t datapoint : imu_data) { - //grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer - const std::int32_t data_value = extract_int24(datapoint); - - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_meas_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_meas_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_meas_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_meas_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - imu_meas_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_meas_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius); - - imu_temp_.header.stamp = callback_time; - imu_temp_pub_->publish(imu_temp_); - break; - // The below are not currently used - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } - imu_meas_pub_->publish(imu_meas_); - } -} - -// -// Decode the Raw IMU measurement output -// -void HpgDrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { - if (getRosBoolean(node_, "publish.esf.raw")) { - esf_raw_pub_->publish(m); - } - imu_raw_.header.stamp = node_->now(); - imu_raw_.header.frame_id = frame_id_; - imu_raw_.orientation_covariance[0] = -1; - imu_raw_.linear_acceleration_covariance[0] = -1; - imu_raw_.angular_velocity_covariance[0] = -1; - - const std::vector imu_data_blocks = m.blocks; - for (const ublox_msgs::msg::EsfRAWBlock &imu_data_entry : imu_data_blocks) { - const std::uint32_t datapoint = imu_data_entry.data; - - // Grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer, and cast to double - const std::int32_t data_value = extract_int24(datapoint); - - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - break; // Do nothing, just catch - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; // Do nothing, just catch - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } - } - imu_raw_pub_->publish(imu_raw_); -} - -// -// Decode the High-Precision Geodetic Position message (HPPOSLLH) into NavSatFix -// -void HpgDrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { - // Do not publish invalid HPPOSLLH data - if (m.flags != 0) { - return; - } - - if (getRosBoolean(node_, "publish.nav.hpposllh")) { - nav_hpposllh_pub_->publish(m); - } - - fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device - - if (last_nav_pvt_.fix_type >= ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { - fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; - } else { - fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; - } - - // Calculate the high-precision lat, lon, alt values - fix_hp_.latitude = 1e-7 * (static_cast(m.lat) + (static_cast(m.lat_hp) * 1e-2)); - fix_hp_.longitude = 1e-7 * (static_cast(m.lon) + (static_cast(m.lon_hp) * 1e-2)); - fix_hp_.altitude = 1e-3 * (static_cast(m.height) + (static_cast(m.height_hp) * 1e-1)); - - // Populate the covariance data - const double var_h = std::pow(m.h_acc / 1000.0, 2); - const double var_v = std::pow(m.v_acc / 1000.0, 2); - fix_hp_.position_covariance[0] = var_h; - fix_hp_.position_covariance[4] = var_h; - fix_hp_.position_covariance[8] = var_v; - fix_hp_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - - fix_hp_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - fix_hp_pub_->publish(fix_hp_); -} - -// -// Use NavPVT for some message fusion diagnostics data -// -void HpgDrProduct::callbackNavPvt(const ublox_msgs::msg::NavPVT& m) { - if (getRosBoolean(node_, "publish.nav.pvt")) { - nav_pvt_pub_->publish(m); - } - last_nav_pvt_ = m; - - // update the iTow 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 - last_itow_time_.first = m.i_tow; - if (m.nano < 0) { - last_itow_time_.second.sec = toUtcSeconds(m) - 1; - last_itow_time_.second.nanosec = static_cast(m.nano + 1e9); - } - else { - last_itow_time_.second.sec = toUtcSeconds(m); - last_itow_time_.second.nanosec = static_cast(m.nano); - } - } -} - - -} // namespace ublox_node From dd84bc260cacc674e1e0ade305e588be87feb4ee Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 18:24:47 -0800 Subject: [PATCH 05/13] Add support for NavHPPOSECEF --- README.md | 9 ++++- ublox_gps/src/adr_udr_product.cpp | 2 +- ublox_mcu-config | 8 ++++ ublox_msgs/CMakeLists.txt | 1 + .../include/ublox_msgs/serialization.hpp | 40 +++++++++++++++++++ ublox_msgs/include/ublox_msgs/ublox_msgs.hpp | 2 + ublox_msgs/msg/NavHPPOSECEF.msg | 25 ++++++++++++ ublox_msgs/msg/NavHPPOSLLH.msg | 1 - ublox_msgs/src/ublox_msgs.cpp | 2 + 9 files changed, 86 insertions(+), 4 deletions(-) create mode 100644 ublox_mcu-config create mode 100644 ublox_msgs/msg/NavHPPOSECEF.msg diff --git a/README.md b/README.md index e5da26ee..949ae2cf 100644 --- a/README.md +++ b/README.md @@ -128,7 +128,11 @@ The `ublox_gps` node supports the following parameters for all products and firm `~fix`([sensor_msgs/NavSatFix](http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html)) -Navigation Satellite fix. +Navigation Satellite fix. + +`~fix_highprecision`([sensor_msgs/NavSatFix](http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html)) + +Navigation Satellite fix. Only for high-precision ADR/UDR devices. `~fix_velocity`([geometry_msgs/TwistWithCovarianceStamped](http://docs.ros.org/jade/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html)) @@ -171,12 +175,13 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below * `publish/nav/all`: This is the default value for the `publish/mon/` parameters below. It defaults to `publish/all`. Individual messages can be enabled or disabled by setting the parameters below. * `publish/nav/att`: Topic `~navatt`. **ADR/UDR devices only** * `publish/nav/clock`: Topic `~navclock` +* `publish/nav/heading`: Topic `~navheading`. **Firmware >= 9 HP Position receiver devices only.** +* `publish/nav/hpposecef`: Topic `~hpposecef` **Firmware >= 8 High-Precision GNSS Devices only**. * `publish/nav/hpposllh`: Topic `~hpposllh` **Firmware >= 8 High-Precision GNSS Devices only**. * `publish/nav/posecef`: Topic `~navposecef` * `publish/nav/posllh`: Topic `~navposllh`. **Firmware <= 6 only.** For firmware 7 and above, see NavPVT * `publish/nav/pvt`: Topic `~navpvt`. **Firmware >= 7 only.** * `publish/nav/relposned`: Topic `~navrelposned`. **HPG Rover devices only** -* `publish/nav/heading`: Topic `~navheading`. **HP Position receiver devices only.** For firmware 9 and above * `publish/nav/sat`: Topic `~navsat` * `publish/nav/sol`: Topic `~navsol`. **Firmware <= 6 only.** For firmware 7 and above, see NavPVT * `publish/nav/status`: Topic `~navstatus` diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 2bbc4117..22fadb3a 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -74,7 +74,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); imu_temp_pub_ = node_->create_publisher("~/imu_temp", 1); esf_ins_ros_pub_ = node_->create_publisher("~/kinematics", 1); - fix_hp_pub_ = node_->create_publisher("~/fix", 1); + fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); // Perform any message metadata value setting we can do only once, including default values diff --git a/ublox_mcu-config b/ublox_mcu-config new file mode 100644 index 00000000..20d34b70 --- /dev/null +++ b/ublox_mcu-config @@ -0,0 +1,8 @@ +# Wheel speed RR +0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x27 0x10 0x91 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x39 0x01 0xe7 0x2d + +# Wheel speed RL +0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x37 0x10 0x92 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x29 0x01 0xe8 0xd7 + +# Direction (Drive gear engaged or not) +0x43 0xa2 0x11 0x13 0x03 0xbc 0x03 0x00 0x00 0x08 0x64 0x2f 0x01 0x00 0x00 0x01 0x00 0x00 0x00 0xe8 0x03 0x5f 0x01 0xce 0xb8 \ No newline at end of file diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 040dfae6..54ff66d5 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -70,6 +70,7 @@ set(msg_files "msg/NavDGPS.msg" "msg/NavDGPSSV.msg" "msg/NavDOP.msg" + "msg/NavHPPOSECEF.msg" "msg/NavHPPOSLLH.msg" "msg/NavPOSECEF.msg" "msg/NavPOSLLH.msg" diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index d7820799..5df0eabc 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1756,6 +1756,46 @@ struct UbloxSerializer > { } }; +template +struct UbloxSerializer > { + inline static void read(const uint8_t *data, uint32_t count, + ublox_msgs::msg::NavHPPOSECEF_ & m) { + UbloxIStream stream(const_cast(data), count); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.ecef_x_hp); + stream.next(m.ecef_y_hp); + stream.next(m.ecef_z_hp); + stream.next(m.flags); + stream.next(m.p_acc); + } + + inline static uint32_t serializedLength(const ublox_msgs::msg::NavHPPOSECEF_ & m) { + (void)m; + return 28; + } + + inline static void write(uint8_t *data, uint32_t size, + const ublox_msgs::msg::NavHPPOSECEF_ & m) { + UbloxOStream stream(data, size); + stream.next(m.version); + stream.next(m.reserved0); + stream.next(m.i_tow); + stream.next(m.ecef_x); + stream.next(m.ecef_y); + stream.next(m.ecef_z); + stream.next(m.ecef_x_hp); + stream.next(m.ecef_y_hp); + stream.next(m.ecef_z_hp); + stream.next(m.flags); + stream.next(m.p_acc); + } +}; + 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 1678e2b5..036847ab 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,7 @@ namespace Message { 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 HPPOSLLH = ublox_msgs::msg::NavHPPOSLLH::MESSAGE_ID; + static const uint8_t HPPOSECEF = ublox_msgs::msg::NavHPPOSECEF::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; diff --git a/ublox_msgs/msg/NavHPPOSECEF.msg b/ublox_msgs/msg/NavHPPOSECEF.msg new file mode 100644 index 00000000..4407338e --- /dev/null +++ b/ublox_msgs/msg/NavHPPOSECEF.msg @@ -0,0 +1,25 @@ +# NAV-HPPOSECEF (0x01 0x13) +# High Precision Position Solution in ECEF +# +# See important comments concerning validity of position given in section +# Navigation Output Filters. +# + +uint8 CLASS_ID = 0x01 +uint8 MESSAGE_ID = 0x13 + +uint8 version +uint8[3] reserved0 + +uint32 i_tow # GPS Millisecond Time of Week [ms] + +int32 ecef_x # ECEF X coordinate [cm] +int32 ecef_y # ECEF Y coordinate [cm] +int32 ecef_z # ECEF Z coordinate [cm] + +int8 ecef_x_hp # ECEF X high precision component [0.1mm] +int8 ecef_y_hp # ECEF Y high precision component [0.1mm] +int8 ecef_z_hp # ECEF Z high precision component [0.1mm] +uint8 flags + +uint32 p_acc # Position Accuracy Estimate [0.1mm] diff --git a/ublox_msgs/msg/NavHPPOSLLH.msg b/ublox_msgs/msg/NavHPPOSLLH.msg index 73f33851..710cbd61 100644 --- a/ublox_msgs/msg/NavHPPOSLLH.msg +++ b/ublox_msgs/msg/NavHPPOSLLH.msg @@ -11,7 +11,6 @@ uint8 CLASS_ID = 0x01 uint8 MESSAGE_ID = 0x14 - uint8 version uint16 reserved1 uint8 flags # bit 0: 1=invalid position, 0 = valid diff --git a/ublox_msgs/src/ublox_msgs.cpp b/ublox_msgs/src/ublox_msgs.cpp index 6a3d1736..99d07669 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -44,6 +44,8 @@ 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, ublox_msgs, NavDOP) +DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSECEF, + ublox_msgs, NavHPPOSECEF) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::HPPOSLLH, ublox_msgs, NavHPPOSLLH) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSECEF, From 43e439c53b9ac02e40fb58d4488bfea009ba66fc Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 21:34:02 -0800 Subject: [PATCH 06/13] Add ZED-F9L to supported ADR/UDR HP list --- .../include/ublox_gps/adr_udr_product.hpp | 14 +- ublox_gps/src/adr_udr_product.cpp | 160 +++++++++++------- ublox_gps/src/node.cpp | 10 +- 3 files changed, 111 insertions(+), 73 deletions(-) diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 5fb5710b..09ef2342 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -12,7 +12,6 @@ #include #include #include -#include #include #include @@ -41,7 +40,7 @@ constexpr float kConvertDegCelsius{0.01F}; */ 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, const std::string & product_model, rclcpp::Node* node); + explicit AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -77,16 +76,14 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning bool use_adr_; + float last_imu_temperature_{0.F}; sensor_msgs::msg::Imu imu_; sensor_msgs::msg::Imu imu_raw_; sensor_msgs::msg::Imu imu_att_; sensor_msgs::msg::Imu esf_ins_ros_; - sensor_msgs::msg::Temperature imu_temp_; - sensor_msgs::msg::TimeReference t_ref_; sensor_msgs::msg::NavSatFix fix_hp_; diagnostic_msgs::msg::DiagnosticStatus nav_diag_; - ublox_msgs::msg::NavHPPOSLLH last_hp_pos_; // Local cache of U-Blox messages to be fused within a navigation epoch (data frame) ublox_msgs::msg::NavATT last_nav_att_; @@ -99,6 +96,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { void callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m); void callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m); void callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH &m); + void callbackNavHpPosEcef(const ublox_msgs::msg::NavHPPOSECEF &m); void callbackNavATT(const ublox_msgs::msg::NavATT &m); void callbackNavPVT(const ublox_msgs::msg::NavPVT &m); @@ -107,15 +105,15 @@ class AdrUdrProduct final : public virtual ComponentInterface { rclcpp::Publisher::SharedPtr imu_raw_pub_; rclcpp::Publisher::SharedPtr imu_att_pub_; rclcpp::Publisher::SharedPtr esf_ins_ros_pub_; - rclcpp::Publisher::SharedPtr imu_temp_pub_; rclcpp::Publisher::SharedPtr fix_hp_pub_; + rclcpp::Publisher::SharedPtr imu_temp_pub_; rclcpp::Publisher::SharedPtr nav_diag_pub_; - rclcpp::Publisher::SharedPtr time_ref_pub_; // Republishers for U-Blox messages rclcpp::Publisher::SharedPtr nav_att_pub_; rclcpp::Publisher::SharedPtr nav_pvt_pub_; rclcpp::Publisher::SharedPtr nav_hpposllh_pub_; + rclcpp::Publisher::SharedPtr nav_hpposecef_pub_; rclcpp::Publisher::SharedPtr esf_ins_pub_; rclcpp::Publisher::SharedPtr esf_meas_pub_; rclcpp::Publisher::SharedPtr esf_raw_pub_; @@ -127,7 +125,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { std::string frame_id_; std::shared_ptr updater_; - std::string product_model_; + bool use_highprecision_; rclcpp::Node* node_; }; diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 22fadb3a..d9cc2490 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -37,22 +37,17 @@ static inline std::int32_t extract_int24(std::uint32_t bitfield) { // U-Blox Automotive or Untethered Dead Reckoning // High Precision GNSS products have have firmware version >= 8 // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const std::string & product_model, rclcpp::Node* node) - : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node), product_model_(product_model) +AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node) + : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), use_highprecision_(use_highprecision), node_(node) { 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.nav.hpposllh")) { - nav_hpposllh_pub_ = node_->create_publisher("navhpposllh", 1); - } if (getRosBoolean(node_, "publish.nav.pvt")) { nav_pvt_pub_ = node_->create_publisher("navpvt", 1); } @@ -68,13 +63,31 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s if (getRosBoolean(node_, "publish.hnr.pvt")) { hnr_pvt_pub_ = node_->create_publisher("hnrpvt", 1); } + if (getRosBoolean(node_, "publish.nav.hpposllh")) { + if (use_highprecision_) { + nav_hpposllh_pub_ = node_->create_publisher("navhpposllh", 1); + } else { + RCLCPP_WARN(node_->get_logger(), + "Parameter 'publish.nav.hpposllh' is enabled, but this device is not recognized as a high-precision product."); + } + } + if (getRosBoolean(node_, "publish.nav.hpposecef")) { + if (use_highprecision_) { + nav_hpposecef_pub_ = node_->create_publisher("navhpposecef", 1); + } else { + RCLCPP_WARN(node_->get_logger(), + "Parameter 'publish.nav.hpposecef' is enabled, but this device is not recognized as a high-precision product."); + } + } imu_pub_ = node_->create_publisher("~/imu_meas", 1); imu_att_pub_ = node_->create_publisher("~/imu_att", 1); imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); - imu_temp_pub_ = node_->create_publisher("~/imu_temp", 1); esf_ins_ros_pub_ = node_->create_publisher("~/kinematics", 1); - fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); + + if (use_highprecision_) { + fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); + } nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); // Perform any message metadata value setting we can do only once, including default values @@ -93,10 +106,6 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s imu_raw_ = imu_; // Initialize using the same values as bove imu_att_ = imu_; // Initialize using the same values as above - imu_temp_.header.frame_id = frame_id_; - imu_temp_.variance = 0.0; // signifies unknown variance - imu_temp_.temperature = 0.0; // signifies missing data - esf_ins_ros_.header.frame_id = frame_id_; esf_ins_ros_.linear_acceleration_covariance[0] = -1.0; // signifies missing data esf_ins_ros_.angular_velocity_covariance[0] = -1.0; // signifies missing data @@ -110,21 +119,24 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; nav_diag_.name = "NavigationDiagnostics"; nav_diag_.message = "EsfSTATUS"; - nav_diag_.hardware_id = product_model_; + nav_diag_.hardware_id = use_highprecision_; } void AdrUdrProduct::subscribe(std::shared_ptr gps) { + if (use_highprecision_) { + // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavHpPosEcef, this, std::placeholders::_1), 1); + } // Subscribe to the Position-Velocity-Time solution messages. // These provide important metadata gps->subscribe(std::bind( &AdrUdrProduct::callbackNavPVT, this, std::placeholders::_1), 1); - // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 - gps->subscribe(std::bind( - &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); - // Sensor Fusion status for diagnostics gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); @@ -217,6 +229,9 @@ void AdrUdrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { last_nav_att_ = m; } +// +// Publish a sensor_msgs/msg/Imu message for the intertial navigation solution +// void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_ros_.header.stamp = node_->now(); @@ -224,41 +239,45 @@ void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); } - // To avoid long mutexing, let's just grab a copy of the last NavATT frame to match for data frame ID - ublox_msgs::msg::NavATT temp_att = last_nav_att_; + // To avoid mutexing, we just grab a copy of the last NavATT frame to match for data frame ID + const ublox_msgs::msg::NavATT nav_att = last_nav_att_; // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT - if (temp_att.i_tow == m.i_tow) { - constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + if (nav_att.i_tow == m.i_tow) { + constexpr double kEsfInsScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - const double roll = M_PI_2 - (static_cast(temp_att.roll) * kNavAttScaleAndRadianConversion); - const double pitch = M_PI_2 - (static_cast(temp_att.pitch) * kNavAttScaleAndRadianConversion); - const double heading = M_PI_2 - (static_cast(temp_att.heading) * kNavAttScaleAndRadianConversion); + const double roll = M_PI_2 - (static_cast(nav_att.roll) * kEsfInsScaleAndRadianConversion); + const double pitch = M_PI_2 - (static_cast(nav_att.pitch) * kEsfInsScaleAndRadianConversion); + const double heading = M_PI_2 - (static_cast(nav_att.heading) * kEsfInsScaleAndRadianConversion); tf2::Quaternion orientation; - orientation.setRPY(roll, pitch, heading); + orientation.setRPY(roll, pitch, heading); // Translate from Euler angles to a Quaternion esf_ins_ros_.orientation.x = orientation[0]; esf_ins_ros_.orientation.y = orientation[1]; esf_ins_ros_.orientation.z = orientation[2]; esf_ins_ros_.orientation.w = orientation[3]; - esf_ins_ros_.orientation_covariance[0] = std::pow(temp_att.acc_roll * kNavAttScaleAndRadianConversion, 2); - esf_ins_ros_.orientation_covariance[4] = std::pow(temp_att.acc_pitch * kNavAttScaleAndRadianConversion, 2); - esf_ins_ros_.orientation_covariance[8] = std::pow(temp_att.acc_heading * kNavAttScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[0] = + std::pow(static_cast(nav_att.acc_roll) * kEsfInsScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[4] = + std::pow(static_cast(nav_att.acc_pitch) * kEsfInsScaleAndRadianConversion, 2); + esf_ins_ros_.orientation_covariance[8] = + std::pow(static_cast(nav_att.acc_heading) * kEsfInsScaleAndRadianConversion, 2); } else { // No data available for this data frame esf_ins_ros_.orientation_covariance[0] = -1.0; esf_ins_ros_.orientation_covariance[4] = -1.0; esf_ins_ros_.orientation_covariance[8] = -1.0; } - constexpr double kMilliGramsToNewtons{1e-6}; + constexpr double kScaleNewtons{1e-6}; constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.x = static_cast(m.x_accel) * kMilliGramsToNewtons; - esf_ins_ros_.angular_velocity.y = static_cast(m.y_accel) * kMilliGramsToNewtons; - esf_ins_ros_.angular_velocity.z = static_cast(m.z_accel) * kMilliGramsToNewtons; + + esf_ins_ros_.linear_acceleration.x = static_cast(m.x_accel) * kScaleNewtons; + esf_ins_ros_.linear_acceleration.y = static_cast(m.y_accel) * kScaleNewtons; + esf_ins_ros_.linear_acceleration.z = static_cast(m.z_accel) * kScaleNewtons; esf_ins_ros_pub_->publish(esf_ins_ros_); } @@ -311,24 +330,24 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; - break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; - break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius; - break; // Reserved for future use + last_imu_temperature_ = static_cast(data_value) * kConvertDegCelsius; + break; // The following are not currently used case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: @@ -342,8 +361,6 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { } imu_.header.stamp = callback_time; imu_pub_->publish(imu_); - imu_temp_.header.stamp = callback_time; - imu_temp_pub_->publish(imu_temp_); } } @@ -351,6 +368,7 @@ void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { // Parse the sensor fusion status information // void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { + const rclcpp::Time callback_time{node_->now()}; diagnostic_msgs::msg::KeyValue wt_status; wt_status.key = "wheel_tick_status"; wt_status.value = wt_status_str(m.reserved1[0]); @@ -369,6 +387,9 @@ void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { diagnostic_msgs::msg::KeyValue num_sens; num_sens.key = "num_sensors"; num_sens.value = std::to_string(m.num_sens); + diagnostic_msgs::msg::KeyValue imu_temp; + imu_temp.key = "IMU_temperature"; + imu_temp.value = std::to_string(last_imu_temperature_); nav_diag_.values.push_back(imu_alg); nav_diag_.values.push_back(imu_ini); @@ -376,9 +397,15 @@ void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { nav_diag_.values.push_back(ins_ini); nav_diag_.values.push_back(fusion_mode); nav_diag_.values.push_back(num_sens); + nav_diag_.values.push_back(imu_temp); nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; nav_diag_pub_->publish(nav_diag_); + sensor_msgs::msg::Temperature temp; + temp.header.stamp = callback_time; + temp.temperature = last_imu_temperature_; + temp.variance = 0.0; + imu_temp_pub_->publish(temp); } @@ -401,20 +428,26 @@ void AdrUdrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { switch (data_type) { case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; break; + imu_raw_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; + break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; break; + imu_raw_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; + break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; break; + imu_raw_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_raw_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_raw_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; break; - + imu_raw_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; + break; case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - imu_temp_.temperature = static_cast(data_value) * kConvertDegCelsius; break; + last_imu_temperature_ = static_cast(data_value) * kConvertDegCelsius; + break; // The following are not currently used; they relate to sensor fusion with wheel encoder data case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: @@ -428,8 +461,15 @@ void AdrUdrProduct::callbackEsfRAW(const ublox_msgs::msg::EsfRAW &m) { } imu_raw_.header.stamp = callback_time; imu_raw_pub_->publish(imu_raw_); - imu_temp_.header.stamp = callback_time; - imu_temp_pub_->publish(imu_temp_); + } +} + +// +// Republish the High-Precision ECEF Position message (HPPOSECEF) +// +void AdrUdrProduct::callbackNavHpPosEcef(const ublox_msgs::msg::NavHPPOSECEF& m) { + if (getRosBoolean(node_, "publish.nav.hpposecef")) { + nav_hpposecef_pub_->publish(m); } } @@ -443,7 +483,7 @@ void AdrUdrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { } if (getRosBoolean(node_, "publish.nav.hpposllh")) { - nav_hpposllh_pub_->publish(m); + nav_hpposllh_pub_->publish(m); } fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device @@ -484,10 +524,10 @@ void AdrUdrProduct::callbackNavPVT(const ublox_msgs::msg::NavPVT& m) { 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 + // Use the NavPVT timestamp since it reflects the device computation time + // The nanosecond time from the NavPVT message can be between -1e9 and 1e9 + // rclcpp::Time uses only unsigned values, so a negative nanosecond value + // must be converted to a positive value last_itow_time_.first = m.i_tow; if (m.nano < 0) { last_itow_time_.second.sec = ublox_node::toUtcSeconds(m) - 1; diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 8aff295a..6e9b06a4 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -229,8 +229,9 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(nav_rate_, updater_, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision GNSS Rovers"); // High-Precision GNSS Sensor Fusion products with Dead Reckoning capability - } else if (product_category == "HPS" && (product_model == "ZED-F9R" || product_model == "ZED-F9K")) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, product_model, this)); + } else if ((product_category == "HPS" && + (product_model == "ZED-F9R" || product_model == "ZED-F9K" || product_model == "ZED-F9L"))) { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, true, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision Dead Reckoning products."); // Any other High-Precision GNSS product, without sensor fusion } else if (product_category == "HPG" || product_category == "HPS") { @@ -241,9 +242,8 @@ void UbloxNode::addProductInterface(const std::string & product_category, components_.push_back(std::make_shared(frame_id_, updater_, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for Time Sync products"); // Automotive Dead Reckoning or Untethered Dead Reckoning products not detected above - } else if (product_category == "ADR" || - product_category == "UDR") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, product_model, this)); + } else if (product_category == "ADR" || product_category == "UDR") { + components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, false, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for Automotive/Untethered Dead Reckoning products"); // Frequency & other Time Synchronization products } else if (product_category == "FTS") { From 839d1da0753e64d98dd717dba1fdb18d9f761b50 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 22:32:32 -0800 Subject: [PATCH 07/13] Clean up redundant code --- README.md | 6 +- ublox_gps/config/zed_f9r.yaml | 28 +++--- .../include/ublox_gps/adr_udr_product.hpp | 19 ++-- ublox_gps/src/adr_udr_product.cpp | 90 ++++++++----------- 4 files changed, 65 insertions(+), 78 deletions(-) diff --git a/README.md b/README.md index 949ae2cf..07738ed1 100644 --- a/README.md +++ b/README.md @@ -195,9 +195,9 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below * `publish/esf/meas`: Topic `~esfmeas` * `publish/esf/raw`: Topic `~esfraw` * `publish/esf/status`: Topic `~esfstatus` -* Topic `~imu`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) -* Topic `~imu_raw`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) - +* Topic `~imu`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfMEAS +* Topic `~imu_raw`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfRAW +* Topic ~imu ### HNR messages * `publish/hnr/pvt`: Topic `~hnrpvt`. **ADR/UDR devices only** diff --git a/ublox_gps/config/zed_f9r.yaml b/ublox_gps/config/zed_f9r.yaml index b196dde1..68d5e58f 100644 --- a/ublox_gps/config/zed_f9r.yaml +++ b/ublox_gps/config/zed_f9r.yaml @@ -1,26 +1,30 @@ # Configuration Settings for the C102-F9R (ZED-F9R) device +# but should also work for ZED-F9K and ZED-F9L devices. ublox_gps_node: ros__parameters: + debug: 1 # Range 0-4 (0 means no debug statements will print) + device: /dev/ttyACM0 + frame_id: "ZED-F9R" + config_on_startup: false # Sufficient device configuration not feasible - # with the current ublox node codebase. Use U-Center instead. + # with the current ublox node codebase. Use U-Center + # instead to flash a persistent configuration. uart1: - baudrate: 115200 + baudrate: 115200 # reasonably high output rate required for 50Hz+ outputs in: 1 # ubx protocol id, see CfgPRT.msg out: 0 # ubx protocol id, see CfgPRT.msg - debug: 3 # Range 0-4 (0 means no debug statements will print) - device: /dev/ttyACM0 - frame_id: zedf9r - inf: - all: false # Whether to display all INF messages in console + all: false # Whether to display all INF messages in console publish: all: false esf: - raw: true - meas: true + raw: true # unprocessed IMU readings + meas: true # postprocessed IMU readings + ins: true # Inertial Navigation System estimate nav: - att: true - hpposllh: true - pvt: true + att: true # vehicle attitude estimate + hpposllh: true # high-precision lat-lon-height + hpposecef: true # high-precision ECEF position + pvt: true # position-velocity-time solution diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 09ef2342..4b12a31f 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -5,9 +5,6 @@ #include #include -#include -#include - #include #include #include @@ -21,10 +18,10 @@ #include #include #include -#include +#include +#include #include -#include namespace ublox_node { @@ -78,12 +75,12 @@ class AdrUdrProduct final : public virtual ComponentInterface { bool use_adr_; float last_imu_temperature_{0.F}; - sensor_msgs::msg::Imu imu_; - sensor_msgs::msg::Imu imu_raw_; - sensor_msgs::msg::Imu imu_att_; - sensor_msgs::msg::Imu esf_ins_ros_; - sensor_msgs::msg::NavSatFix fix_hp_; - diagnostic_msgs::msg::DiagnosticStatus nav_diag_; + sensor_msgs::msg::Imu imu_{}; + sensor_msgs::msg::Imu imu_raw_{}; + sensor_msgs::msg::Imu imu_att_{}; + sensor_msgs::msg::Imu esf_ins_ros_{}; + sensor_msgs::msg::NavSatFix fix_hp_{}; + diagnostic_msgs::msg::DiagnosticStatus nav_diag_{}; // Local cache of U-Blox messages to be fused within a navigation epoch (data frame) ublox_msgs::msg::NavATT last_nav_att_; diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index d9cc2490..9e6ca281 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -26,7 +25,7 @@ namespace ublox_node { // // Extract U-Blox 24-bit signed integers from a 32-bit data blob -// and cast into int32_t +// and cast to int32_t // static inline std::int32_t extract_int24(std::uint32_t bitfield) { std::int32_t temp = static_cast(bitfield << 8); @@ -47,15 +46,18 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s } if (getRosBoolean(node_, "publish.nav.att")) { nav_att_pub_ = node_->create_publisher("navatt", 1); + imu_att_pub_ = node_->create_publisher("~/imu_att", 1); } if (getRosBoolean(node_, "publish.nav.pvt")) { nav_pvt_pub_ = node_->create_publisher("navpvt", 1); } if (getRosBoolean(node_, "publish.esf.ins")) { esf_ins_pub_ = node_->create_publisher("esfins", 1); + esf_ins_ros_pub_ = node_->create_publisher("~/kinematics", 1); } if (getRosBoolean(node_, "publish.esf.raw")) { esf_raw_pub_ = node_->create_publisher("esfraw", 1); + imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); } if (getRosBoolean(node_, "publish.esf.status")) { esf_status_pub_ = node_->create_publisher("esfstatus", 1); @@ -79,15 +81,12 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s "Parameter 'publish.nav.hpposecef' is enabled, but this device is not recognized as a high-precision product."); } } - - imu_pub_ = node_->create_publisher("~/imu_meas", 1); - imu_att_pub_ = node_->create_publisher("~/imu_att", 1); - imu_raw_pub_ = node_->create_publisher("~/imu_raw", 1); - esf_ins_ros_pub_ = node_->create_publisher("~/kinematics", 1); - if (use_highprecision_) { fix_hp_pub_ = node_->create_publisher("~/fix_highprecision", 1); } + if (getRosBoolean(node_, "publish.esf.meas") || getRosBoolean(node_, "publish.esf.raw")) { + imu_temp_pub_ = node_->create_publisher("~/imu_temperature", 1); + } nav_diag_pub_ = node_->create_publisher("~/fusion_status", 1); // Perform any message metadata value setting we can do only once, including default values @@ -105,10 +104,7 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s imu_raw_ = imu_; // Initialize using the same values as bove imu_att_ = imu_; // Initialize using the same values as above - - esf_ins_ros_.header.frame_id = frame_id_; - esf_ins_ros_.linear_acceleration_covariance[0] = -1.0; // signifies missing data - esf_ins_ros_.angular_velocity_covariance[0] = -1.0; // signifies missing data + esf_ins_ros_ = imu_; // Initialize using the same values as above fix_hp_.header.frame_id = frame_id_; fix_hp_.position_covariance[0] = -1.0; @@ -119,25 +115,23 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::STALE; nav_diag_.name = "NavigationDiagnostics"; nav_diag_.message = "EsfSTATUS"; - nav_diag_.hardware_id = use_highprecision_; + nav_diag_.hardware_id = frame_id_; } - void AdrUdrProduct::subscribe(std::shared_ptr gps) { + // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 if (use_highprecision_) { - // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 gps->subscribe(std::bind( &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); gps->subscribe(std::bind( &AdrUdrProduct::callbackNavHpPosEcef, this, std::placeholders::_1), 1); } // Subscribe to the Position-Velocity-Time solution messages. - // These provide important metadata gps->subscribe(std::bind( &AdrUdrProduct::callbackNavPVT, this, std::placeholders::_1), 1); - // Sensor Fusion status for diagnostics + // Sensor Fusion status for diagnostics output gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); @@ -173,8 +167,9 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { // Subscribe to High-Navigation rate PVT messages if (getRosBoolean(node_, "publish.hnr.pvt")) { - gps->subscribe([this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, - 1); + gps->subscribe( + [this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, + 1); } } @@ -197,18 +192,13 @@ bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { } void AdrUdrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { - ublox_msgs::msg::NavATT temp_att = last_nav_att_; - if (temp_att.i_tow == m.i_tow) { - esf_ins_ros_.header.stamp.sec = last_itow_time_.second.sec; - esf_ins_ros_.header.stamp.nanosec = last_itow_time_.second.nanosec; - imu_att_.header.stamp = node_->now(); - } + imu_att_.header.stamp = node_->now(); if (getRosBoolean(node_, "publish.nav.att")) { nav_att_pub_->publish(m); } + last_nav_att_ = m; constexpr double kNavAttScaleAndRadianConversion{1e-5 * M_PI / 180.0}; - // Transform U-Blox Euler angles to a Quaternion and populate covariances const double roll = M_PI_2 - (static_cast(m.roll) * kNavAttScaleAndRadianConversion); const double pitch = M_PI_2 - (static_cast(m.pitch) * kNavAttScaleAndRadianConversion); @@ -226,7 +216,6 @@ void AdrUdrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { imu_att_.orientation_covariance[8] = std::pow(static_cast(m.acc_heading) * kNavAttScaleAndRadianConversion, 2.0); imu_att_pub_->publish(imu_att_); - last_nav_att_ = m; } // @@ -239,9 +228,20 @@ void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { esf_ins_pub_->publish(m); } - // To avoid mutexing, we just grab a copy of the last NavATT frame to match for data frame ID + constexpr double kScaleNewtons{1e-6}; + constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; + + esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; + esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; + + esf_ins_ros_.linear_acceleration.x = static_cast(m.x_accel) * kScaleNewtons; + esf_ins_ros_.linear_acceleration.y = static_cast(m.y_accel) * kScaleNewtons; + esf_ins_ros_.linear_acceleration.z = static_cast(m.z_accel) * kScaleNewtons; + + // EsfINS does not contain all the data we want, so we use + // the last NavATT message for orientation if its iTOW frame matches this one const ublox_msgs::msg::NavATT nav_att = last_nav_att_; - // If the last NavATT (orientation) message's data frame ID matches that of EsfINS, include the orientation from NavATT if (nav_att.i_tow == m.i_tow) { constexpr double kEsfInsScaleAndRadianConversion{1e-5 * M_PI / 180.0}; @@ -250,7 +250,6 @@ void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { const double heading = M_PI_2 - (static_cast(nav_att.heading) * kEsfInsScaleAndRadianConversion); tf2::Quaternion orientation; orientation.setRPY(roll, pitch, heading); // Translate from Euler angles to a Quaternion - esf_ins_ros_.orientation.x = orientation[0]; esf_ins_ros_.orientation.y = orientation[1]; esf_ins_ros_.orientation.z = orientation[2]; @@ -262,23 +261,11 @@ void AdrUdrProduct::callbackEsfIns(const ublox_msgs::msg::EsfINS &m) { std::pow(static_cast(nav_att.acc_pitch) * kEsfInsScaleAndRadianConversion, 2); esf_ins_ros_.orientation_covariance[8] = std::pow(static_cast(nav_att.acc_heading) * kEsfInsScaleAndRadianConversion, 2); - } else { // No data available for this data frame + } else { // No data available for this data frame, mark as invalid esf_ins_ros_.orientation_covariance[0] = -1.0; esf_ins_ros_.orientation_covariance[4] = -1.0; esf_ins_ros_.orientation_covariance[8] = -1.0; } - - constexpr double kScaleNewtons{1e-6}; - constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; - - esf_ins_ros_.angular_velocity.x = static_cast(m.x_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.y = static_cast(m.y_ang_rate) * kScaleAndRadianConversion; - esf_ins_ros_.angular_velocity.z = static_cast(m.z_ang_rate) * kScaleAndRadianConversion; - - esf_ins_ros_.linear_acceleration.x = static_cast(m.x_accel) * kScaleNewtons; - esf_ins_ros_.linear_acceleration.y = static_cast(m.y_accel) * kScaleNewtons; - esf_ins_ros_.linear_acceleration.z = static_cast(m.z_accel) * kScaleNewtons; - esf_ins_ros_pub_->publish(esf_ins_ros_); } @@ -387,9 +374,6 @@ void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { diagnostic_msgs::msg::KeyValue num_sens; num_sens.key = "num_sensors"; num_sens.value = std::to_string(m.num_sens); - diagnostic_msgs::msg::KeyValue imu_temp; - imu_temp.key = "IMU_temperature"; - imu_temp.value = std::to_string(last_imu_temperature_); nav_diag_.values.push_back(imu_alg); nav_diag_.values.push_back(imu_ini); @@ -397,15 +381,17 @@ void AdrUdrProduct::callbackEsfStatus(const ublox_msgs::msg::EsfSTATUS &m) { nav_diag_.values.push_back(ins_ini); nav_diag_.values.push_back(fusion_mode); nav_diag_.values.push_back(num_sens); - nav_diag_.values.push_back(imu_temp); nav_diag_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; nav_diag_pub_->publish(nav_diag_); - sensor_msgs::msg::Temperature temp; - temp.header.stamp = callback_time; - temp.temperature = last_imu_temperature_; - temp.variance = 0.0; - imu_temp_pub_->publish(temp); + + if (getRosBoolean(node_, "publish.esf.meas") || getRosBoolean(node_, "publish.esf.raw")) { + sensor_msgs::msg::Temperature temp; + temp.header.stamp = callback_time; + temp.temperature = last_imu_temperature_; + temp.variance = 0.0; + imu_temp_pub_->publish(temp); + } } From 243489995505d29843873f4412260f90c366867c Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 22:45:31 -0800 Subject: [PATCH 08/13] Fix high-precision position conversion logic --- README.md | 9 ++++----- ublox_gps/src/adr_udr_product.cpp | 11 +++++------ 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 07738ed1..3fb00a54 100644 --- a/README.md +++ b/README.md @@ -192,12 +192,11 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below ### ESF messages * `publish/esf/all`: This is the default value for the `publish/esf/` parameters below. It defaults to `publish/all` for **ADR/UDR devices**. Individual messages can be enabled or disabled by setting the parameters below. * `publish/esf/ins`: Topic `~esfins` -* `publish/esf/meas`: Topic `~esfmeas` -* `publish/esf/raw`: Topic `~esfraw` +* `publish/esf/meas`: Topic `~esfmeas` and `~imu`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfMEAS +* `publish/esf/raw`: Topic `~esfraw` and `~imu_raw`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfRAW * `publish/esf/status`: Topic `~esfstatus` -* Topic `~imu`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfMEAS -* Topic `~imu_raw`([sensor_msgs/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html)) derived from EsfRAW -* Topic ~imu +* Topic `~imu_temperature`([sensor_msgs/Temperature](https://docs.ros2.org/latest/api/sensor_msgs/msg/Temperature.html)) if either EsfMEAS or EsfRAW are used + ### HNR messages * `publish/hnr/pvt`: Topic `~hnrpvt`. **ADR/UDR devices only** diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 9e6ca281..13d6c948 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -463,6 +463,7 @@ void AdrUdrProduct::callbackNavHpPosEcef(const ublox_msgs::msg::NavHPPOSECEF& m) // Decode the High-Precision Geodetic Position message (HPPOSLLH) into NavSatFix // void AdrUdrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { + fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device // Do not publish invalid HPPOSLLH data if (m.flags != 0) { return; @@ -472,8 +473,6 @@ void AdrUdrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { nav_hpposllh_pub_->publish(m); } - fix_hp_.header.stamp = node_->now(); // Ideally, we should get a timestamp from the device - if (last_nav_pvt_.fix_type >= ublox_msgs::msg::NavPVT::FIX_TYPE_2D) { fix_hp_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; } else { @@ -481,13 +480,13 @@ void AdrUdrProduct::callbackNavHpPosLlh(const ublox_msgs::msg::NavHPPOSLLH& m) { } // Calculate the high-precision lat, lon, alt values - fix_hp_.latitude = 1e-7 * (static_cast(m.lat) + (static_cast(m.lat_hp) * 1e-2)); - fix_hp_.longitude = 1e-7 * (static_cast(m.lon) + (static_cast(m.lon_hp) * 1e-2)); + fix_hp_.latitude = 1e-7 * (static_cast(m.lat) + (static_cast(m.lat_hp) * 1e-9)); + fix_hp_.longitude = 1e-7 * (static_cast(m.lon) + (static_cast(m.lon_hp) * 1e-9)); fix_hp_.altitude = 1e-3 * (static_cast(m.height) + (static_cast(m.height_hp) * 1e-1)); // Populate the covariance data - const double var_h = std::pow(m.h_acc / 1000.0, 2); - const double var_v = std::pow(m.v_acc / 1000.0, 2); + const double var_h = std::pow(static_cast(m.h_acc) * 1e-3, 2.0); + const double var_v = std::pow(static_cast(m.v_acc) * 1e-3, 2.0); fix_hp_.position_covariance[0] = var_h; fix_hp_.position_covariance[4] = var_h; fix_hp_.position_covariance[8] = var_v; From 4c4bb76502281e4eb2a2361e86f63fd8362ba534 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 22:57:17 -0800 Subject: [PATCH 09/13] Clean up diff to master --- ublox_gps/src/adr_udr_product.cpp | 128 +++++++++++++++--------------- 1 file changed, 62 insertions(+), 66 deletions(-) diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 13d6c948..0c28e475 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -120,21 +120,6 @@ AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::s void AdrUdrProduct::subscribe(std::shared_ptr gps) { - // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 - if (use_highprecision_) { - gps->subscribe(std::bind( - &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); - gps->subscribe(std::bind( - &AdrUdrProduct::callbackNavHpPosEcef, this, std::placeholders::_1), 1); - } - // Subscribe to the Position-Velocity-Time solution messages. - gps->subscribe(std::bind( - &AdrUdrProduct::callbackNavPVT, this, std::placeholders::_1), 1); - - // Sensor Fusion status for diagnostics output - gps->subscribe(std::bind( - &AdrUdrProduct::callbackEsfStatus, this, std::placeholders::_1), 1); - // Subscribe to ADR/UDR Navigation Attitude messages if (getRosBoolean(node_, "publish.nav.att")) { gps->subscribe(std::bind( @@ -153,7 +138,7 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); } - // Subscribe to ADR/UDR Raw IMU messages + // Subscribe to ADR/UDR Raw IMU messages if (getRosBoolean(node_, "publish.esf.raw")) { gps->subscribe(std::bind( &AdrUdrProduct::callbackEsfRAW, this, std::placeholders::_1), 1); @@ -171,8 +156,69 @@ void AdrUdrProduct::subscribe(std::shared_ptr gps) { [this](const ublox_msgs::msg::HnrPVT &m) { hnr_pvt_pub_->publish(m); }, 1); } + + // Subscribe to High-Precision Lat-Lon-Height messages; only in firmware >= 8 + if (use_highprecision_) { + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavHpPosLlh, this, std::placeholders::_1), 1); + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavHpPosEcef, this, std::placeholders::_1), 1); + } + // Subscribe to the Position-Velocity-Time solution messages. + gps->subscribe(std::bind( + &AdrUdrProduct::callbackNavPVT, this, std::placeholders::_1), 1); + } +void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { + const rclcpp::Time callback_time{node_->now()}; + // This is time-critical data, so if something is expecting it, republish early + if (getRosBoolean(node_, "publish.esf.meas")) { + esf_meas_pub_->publish(m); + } + + for (const std::uint32_t datapoint : m.data) { + //grab the last six bits of data as the data type description field + const std::uint8_t data_type = datapoint >> 24; + // Interpret the first 24 bits as a signed integer + const std::int32_t data_value = extract_int24(datapoint); + switch (data_type) { + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: + imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: + imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: + imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: + imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: + imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: + imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; + break; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + last_imu_temperature_ = static_cast(data_value) * kConvertDegCelsius; + break; + // The following are not currently used + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: + break; + default: + RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); + } + imu_.header.stamp = callback_time; + imu_pub_->publish(imu_); + } +} void AdrUdrProduct::getRosParams() { use_adr_ = getRosBoolean(node_, "use_adr"); @@ -301,56 +347,6 @@ const char* imu_init_str(std::uint8_t bitfield) { return "deserialization error"; } -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { - const rclcpp::Time callback_time{node_->now()}; - // This is time-critical data, so if something is expecting it, republish early - if (getRosBoolean(node_, "publish.esf.meas")) { - esf_meas_pub_->publish(m); - } - - for (const std::uint32_t datapoint : m.data) { - //grab the last six bits of data as the data type description field - const std::uint8_t data_type = datapoint >> 24; - // Interpret the first 24 bits as a signed integer - const std::int32_t data_value = extract_int24(datapoint); - switch (data_type) { - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_X: - imu_.angular_velocity.x = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Y: - imu_.angular_velocity.y = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_ANG_RATE_Z: - imu_.angular_velocity.z = static_cast(data_value) * kConvertRadPerSec; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_X: - imu_.linear_acceleration.x = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Y: - imu_.linear_acceleration.y = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_ACCELEROMETER_Z: - imu_.linear_acceleration.z = static_cast(data_value) * kConvertMps2; - break; - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: - last_imu_temperature_ = static_cast(data_value) * kConvertDegCelsius; - break; - // The following are not currently used - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_FRONT_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_LEFT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_WHEEL_TICKS_REAR_RIGHT: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SINGLE_TICK: - case ublox_msgs::msg::EsfMEAS::DATA_TYPE_SPEED: - break; - default: - RCLCPP_INFO(node_->get_logger(), "Unknown IMU measurement, data_type: %u , data_value: %d", data_type, data_value); - } - imu_.header.stamp = callback_time; - imu_pub_->publish(imu_); - } -} - // // Parse the sensor fusion status information // From 4a457ba81d2094540152f33a8890bad8859ec298 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 23:03:20 -0800 Subject: [PATCH 10/13] Further cleanup --- ublox_mcu-config | 8 -------- ublox_msgs/CMakeLists.txt | 4 ++-- 2 files changed, 2 insertions(+), 10 deletions(-) delete mode 100644 ublox_mcu-config diff --git a/ublox_mcu-config b/ublox_mcu-config deleted file mode 100644 index 20d34b70..00000000 --- a/ublox_mcu-config +++ /dev/null @@ -1,8 +0,0 @@ -# Wheel speed RR -0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x27 0x10 0x91 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x39 0x01 0xe7 0x2d - -# Wheel speed RL -0x43 0xa2 0x11 0x13 0x03 0xaa 0x00 0x00 0x00 0x08 0x14 0x37 0x10 0x92 0xe5 0x00 0x00 0xa8 0x61 0x0a 0x00 0x29 0x01 0xe8 0xd7 - -# Direction (Drive gear engaged or not) -0x43 0xa2 0x11 0x13 0x03 0xbc 0x03 0x00 0x00 0x08 0x64 0x2f 0x01 0x00 0x00 0x01 0x00 0x00 0x00 0xe8 0x03 0x5f 0x01 0xce 0xb8 \ No newline at end of file diff --git a/ublox_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 54ff66d5..c3383a49 100644 --- a/ublox_msgs/CMakeLists.txt +++ b/ublox_msgs/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(ublox_msgs) -set(LEGACY_ROS_DISTROS "foxy" "galactic" "eloquent" "dashing" "crystal" "bouncy" "ardent" "beta3" "beta2" "beta1") +set(LEGACY_ROS2_DISTROS "foxy" "galactic" "eloquent" "dashing" "crystal" "bouncy" "ardent" "beta3" "beta2" "beta1") # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -107,7 +107,7 @@ set(msg_files "msg/UpdSOS.msg" ) -if(NOT "$ENV{ROS_DISTRO}" IN_LIST LEGACY_ROS_DISTROS) +if(NOT "$ENV{ROS_DISTRO}" IN_LIST LEGACY_ROS2_DISTROS) set(USE_LEGACY_ROSIDL_TARGET "OFF") else() message(STATUS "Using legacy rosidl cmake target interface") From 224ddc0c085f869bba38c0ed9f18e7d58eed6217 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 23:14:56 -0800 Subject: [PATCH 11/13] Synchronize with master --- ublox_gps/include/ublox_gps/adr_udr_product.hpp | 3 ++- ublox_gps/src/adr_udr_product.cpp | 2 +- ublox_gps/src/node.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/ublox_gps/include/ublox_gps/adr_udr_product.hpp b/ublox_gps/include/ublox_gps/adr_udr_product.hpp index 4b12a31f..51690cea 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -37,7 +37,7 @@ constexpr float kConvertDegCelsius{0.01F}; */ 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, const bool use_highprecision, 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, const bool use_highprecision, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -73,6 +73,7 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning bool use_adr_; + float protocol_version_{0.F}; float last_imu_temperature_{0.F}; sensor_msgs::msg::Imu imu_{}; diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index 0c28e475..a69050d1 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -37,7 +37,7 @@ static inline std::int32_t extract_int24(std::uint32_t bitfield) { // High Precision GNSS products have have firmware version >= 8 // AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node) - : use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), use_highprecision_(use_highprecision), node_(node) + : protocol_version_(protocol_version), use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), use_highprecision_(use_highprecision), node_(node) { if (getRosBoolean(node_, "publish.esf.meas")) { imu_pub_ = diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 6e9b06a4..0c1523de 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -231,7 +231,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, // High-Precision GNSS Sensor Fusion products with Dead Reckoning capability } else if ((product_category == "HPS" && (product_model == "ZED-F9R" || product_model == "ZED-F9K" || product_model == "ZED-F9L"))) { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, true, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, true, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for High-Precision Dead Reckoning products."); // Any other High-Precision GNSS product, without sensor fusion } else if (product_category == "HPG" || product_category == "HPS") { @@ -243,7 +243,7 @@ void UbloxNode::addProductInterface(const std::string & product_category, RCLCPP_INFO(this->get_logger(), "Registered product interface for Time Sync products"); // Automotive Dead Reckoning or Untethered Dead Reckoning products not detected above } else if (product_category == "ADR" || product_category == "UDR") { - components_.push_back(std::make_shared(nav_rate_, meas_rate_, frame_id_, updater_, false, this)); + components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, false, this)); RCLCPP_INFO(this->get_logger(), "Registered product interface for Automotive/Untethered Dead Reckoning products"); // Frequency & other Time Synchronization products } else if (product_category == "FTS") { From 0ec73c31011bebe80cb2fa8a0ecd962e3cacc6c7 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 23:41:38 -0800 Subject: [PATCH 12/13] Change Quaternion.h to Quaternion.hpp for version compatibility, fix init order of AdrUdr constructor --- ublox_gps/include/ublox_gps/adr_udr_product.hpp | 4 ++-- ublox_gps/src/adr_udr_product.cpp | 6 +++--- 2 files changed, 5 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 7402b2bf..20503dca 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -72,8 +72,8 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning - bool use_adr_; float protocol_version_{0.F}; + bool use_adr_; float last_imu_temperature_{0.F}; sensor_msgs::msg::Imu imu_{}; @@ -129,4 +129,4 @@ class AdrUdrProduct final : public virtual ComponentInterface { } // namespace ublox_node -#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP \ No newline at end of file +#endif // UBLOX_GPS_ADR_UDR_PRODUCT_HPP diff --git a/ublox_gps/src/adr_udr_product.cpp b/ublox_gps/src/adr_udr_product.cpp index fa37af47..eb0870c9 100644 --- a/ublox_gps/src/adr_udr_product.cpp +++ b/ublox_gps/src/adr_udr_product.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ static inline std::int32_t extract_int24(std::uint32_t bitfield) { // U-Blox Automotive or Untethered Dead Reckoning // High Precision GNSS products have have firmware version >= 8 // -AdrUdrProduct::AdrUdrProduct(uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node) +AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node) : protocol_version_(protocol_version), use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), use_highprecision_(use_highprecision), node_(node) { if (getRosBoolean(node_, "publish.esf.meas")) { @@ -230,7 +230,7 @@ void AdrUdrProduct::getRosParams() { } bool AdrUdrProduct::configureUblox(std::shared_ptr gps) { - if (!gps->setUseAdr(use_adr_, protocol_version_)) { + if (!gps->setUseAdr(use_adr_)) { throw std::runtime_error(std::string("Failed to ") + (use_adr_ ? "enable" : "disable") + "use_adr"); } From e0d530cb2913b1c3bde17de52bec444343182438 Mon Sep 17 00:00:00 2001 From: Lilja Tamminen Date: Sun, 2 Nov 2025 23:47:41 -0800 Subject: [PATCH 13/13] Further clean up diff --- .../include/ublox_serialization/serialization.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ublox_serialization/include/ublox_serialization/serialization.hpp b/ublox_serialization/include/ublox_serialization/serialization.hpp index 0617d23b..f86baddd 100644 --- a/ublox_serialization/include/ublox_serialization/serialization.hpp +++ b/ublox_serialization/include/ublox_serialization/serialization.hpp @@ -31,7 +31,6 @@ #include #include -#include #include #include #include