diff --git a/README.md b/README.md index 65bce136..c2224579 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. @@ -127,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)) @@ -170,11 +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` @@ -187,9 +194,10 @@ 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_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/CMakeLists.txt b/ublox_gps/CMakeLists.txt index feb23f02..281df504 100644 --- a/ublox_gps/CMakeLists.txt +++ b/ublox_gps/CMakeLists.txt @@ -50,6 +50,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/config/zed_f9r.yaml b/ublox_gps/config/zed_f9r.yaml new file mode 100644 index 00000000..68d5e58f --- /dev/null +++ b/ublox_gps/config/zed_f9r.yaml @@ -0,0 +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 to flash a persistent configuration. + uart1: + 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 + + inf: + all: false # Whether to display all INF messages in console + + publish: + all: false + esf: + raw: true # unprocessed IMU readings + meas: true # postprocessed IMU readings + ins: true # Inertial Navigation System estimate + nav: + 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 b00022df..20503dca 100644 --- a/ublox_gps/include/ublox_gps/adr_udr_product.hpp +++ b/ublox_gps/include/ublox_gps/adr_udr_product.hpp @@ -3,11 +3,13 @@ #include #include +#include #include #include #include -#include +#include +#include #include #include @@ -15,20 +17,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(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node); + explicit AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, const bool use_highprecision, rclcpp::Node* node); /** * @brief Get the ADR/UDR parameters. @@ -63,17 +72,46 @@ class AdrUdrProduct final : public virtual ComponentInterface { private: //! Whether or not to enable dead reckoning + float protocol_version_{0.F}; bool use_adr_; - float protocol_version_; - - sensor_msgs::msg::Imu imu_; - sensor_msgs::msg::TimeReference t_ref_; - + 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_{}; + + // 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 callbackNavHpPosEcef(const ublox_msgs::msg::NavHPPOSECEF &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 time_ref_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 imu_temp_pub_; + rclcpp::Publisher::SharedPtr nav_diag_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_; @@ -85,9 +123,10 @@ class AdrUdrProduct final : public virtual ComponentInterface { std::string frame_id_; std::shared_ptr updater_; + bool use_highprecision_; rclcpp::Node* node_; }; } // 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/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/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 7713ba7c..eb0870c9 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,7 +9,7 @@ #include #include #include -#include +#include #include #include @@ -23,27 +24,40 @@ namespace ublox_node { // -// u-blox ADR devices, partially implemented +// Extract U-Blox 24-bit signed integers from a 32-bit data blob +// and cast to int32_t // -AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t meas_rate, const std::string & frame_id, std::shared_ptr updater, rclcpp::Node* node) - : protocol_version_(protocol_version) ,use_adr_(false), nav_rate_(nav_rate), meas_rate_(meas_rate), frame_id_(frame_id), updater_(updater), node_(node) +static inline std::int32_t extract_int24(std::uint32_t bitfield) { + std::int32_t temp = static_cast(bitfield << 8); + return temp >> 8; +} + +// +// U-Blox Automotive or Untethered Dead Reckoning +// High Precision GNSS products have have firmware version >= 8 +// +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")) { 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); + 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); @@ -51,6 +65,159 @@ AdrUdrProduct::AdrUdrProduct(float protocol_version, uint16_t nav_rate, uint16_t 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."); + } + } + 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 + // 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 + 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; + 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 = frame_id_; +} + +void AdrUdrProduct::subscribe(std::shared_ptr gps) { + + // 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); + } + + // 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() { @@ -63,145 +230,295 @@ 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"); } return true; } -void AdrUdrProduct::subscribe(std::shared_ptr gps) { - // Subscribe to NAV ATT messages +void AdrUdrProduct::callbackNavATT(const ublox_msgs::msg::NavATT &m) { + 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); } + 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); + 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_); +} + +// +// 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(); - // Subscribe to ESF INS messages 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); + constexpr double kScaleNewtons{1e-6}; + constexpr double kScaleAndRadianConversion{1e-3 * M_PI / 180.0}; - // also publish sensor_msgs::Imu - gps->subscribe(std::bind( - &AdrUdrProduct::callbackEsfMEAS, this, std::placeholders::_1), 1); + 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 (nav_att.i_tow == m.i_tow) { + constexpr double kEsfInsScaleAndRadianConversion{1e-5 * M_PI / 180.0}; + + 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); // 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(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, 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; } + 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"; +} - // Subscribe to ESF Raw messages +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"; +} + +// +// 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]); + 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_); + + 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); + } +} + + +// +// 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")) { - gps->subscribe([this](const ublox_msgs::msg::EsfRAW &m) { esf_raw_pub_->publish(m); }, - 1); + esf_raw_pub_->publish(m); } - // 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); + 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_GYRO_ANG_RATE_Y: + 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; + 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; + case ublox_msgs::msg::EsfMEAS::DATA_TYPE_GYRO_TEMPERATURE: + 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: + 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_); } +} - // 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); +// +// 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); } } -void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::msg::EsfMEAS &m) { - if (getRosBoolean(node_, "publish.esf.meas")) { - imu_.header.stamp = node_->now(); - imu_.header.frame_id = frame_id_; - - float 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) { - 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; - } - - // 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); - } - - // 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_.time_ref = rclcpp::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR); - - //std::ostringstream src; - //src << "TIM" << int(m.ch); - //t_ref_.source = src.str(); - - t_ref_.header.stamp = node_->now(); // create a new timestamp - t_ref_.header.frame_id = frame_id_; - - time_ref_pub_->publish(t_ref_); - imu_pub_->publish(imu_); - } +// +// 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; + } + + if (getRosBoolean(node_, "publish.nav.hpposllh")) { + nav_hpposllh_pub_->publish(m); + } + + 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-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(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; + 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 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; + 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); + } + } } } // namespace ublox_node diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp index 39c14821..ae2f8dea 100644 --- a/ublox_gps/src/node.cpp +++ b/ublox_gps/src/node.cpp @@ -218,27 +218,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" || product_model == "ZED-F9L"))) { + 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") { 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)); - } else if (product_category == "ADR" || - product_category == "UDR") { - components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, 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(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") { components_.push_back(std::make_shared()); - } else if (product_category == "HPS") { - components_.push_back(std::make_shared(protocol_version_, nav_rate_, meas_rate_, frame_id_, updater_, this)); - components_.push_back(std::make_shared(nav_rate_, updater_, this)); + 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 +429,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")); @@ -689,6 +704,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 @@ -696,10 +714,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; } @@ -708,6 +725,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") @@ -730,6 +748,7 @@ void UbloxNode::processMonVer() { } } } + addProductInterface(product_category, rov_ref, product_model); } } @@ -909,8 +928,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_msgs/CMakeLists.txt b/ublox_msgs/CMakeLists.txt index 4e3db305..c3383a49 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_ROS2_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) @@ -66,6 +70,8 @@ set(msg_files "msg/NavDGPS.msg" "msg/NavDGPSSV.msg" "msg/NavDOP.msg" + "msg/NavHPPOSECEF.msg" + "msg/NavHPPOSLLH.msg" "msg/NavPOSECEF.msg" "msg/NavPOSLLH.msg" "msg/NavPVT7.msg" @@ -101,39 +107,52 @@ set(msg_files "msg/UpdSOS.msg" ) +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") + 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() diff --git a/ublox_msgs/include/ublox_msgs/serialization.hpp b/ublox_msgs/include/ublox_msgs/serialization.hpp index 2ce349e4..f7e587f2 100644 --- a/ublox_msgs/include/ublox_msgs/serialization.hpp +++ b/ublox_msgs/include/ublox_msgs/serialization.hpp @@ -1756,6 +1756,92 @@ 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, + 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..036847ab 100644 --- a/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp +++ b/ublox_msgs/include/ublox_msgs/ublox_msgs.hpp @@ -34,6 +34,8 @@ #include #include #include +#include +#include #include #include #include @@ -162,6 +164,8 @@ 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 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/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/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 new file mode 100644 index 00000000..710cbd61 --- /dev/null +++ b/ublox_msgs/msg/NavHPPOSLLH.msg @@ -0,0 +1,31 @@ +# 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..99d07669 100644 --- a/ublox_msgs/src/ublox_msgs.cpp +++ b/ublox_msgs/src/ublox_msgs.cpp @@ -39,11 +39,15 @@ 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::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, ublox_msgs, NavPOSECEF) DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::NAV, ublox_msgs::Message::NAV::POSLLH,