Add support for 9th gen ZED-series ADR/UDR products

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/<message>` 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/<message>` 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**

ublox_gps/CMakeLists.txt

@@ -50,6 +50,8 @@ target_include_directories(ublox_gps PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
   ${diagnostic_updater_INCLUDE_DIRS}
+  ${ublox_msgs_INCLUDE_DIRS}
+  ${ublox_serialization_INCLUDE_DIRS}
 )
 target_link_libraries(ublox_gps PUBLIC
   ${asio_LIBRARIES}

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

ublox_gps/include/ublox_gps/adr_udr_product.hpp

@@ -3,32 +3,41 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/time_reference.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <sensor_msgs/msg/temperature.hpp>
 
 #include <ublox_msgs/msg/esf_ins.hpp>
 #include <ublox_msgs/msg/esf_meas.hpp>
 #include <ublox_msgs/msg/esf_raw.hpp>
 #include <ublox_msgs/msg/esf_status.hpp>
 #include <ublox_msgs/msg/hnr_pvt.hpp>
 #include <ublox_msgs/msg/nav_att.hpp>
-#include <ublox_msgs/msg/tim_tm2.hpp>
+#include <ublox_msgs/msg/nav_hpposllh.hpp>
+#include <ublox_msgs/msg/nav_hpposecef.hpp>
 
 #include <ublox_gps/component_interface.hpp>
 #include <ublox_gps/gps.hpp>
 
 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<diagnostic_updater::Updater> 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<diagnostic_updater::Updater> 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<std::uint32_t, builtin_interfaces::msg::Time> 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<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
-  rclcpp::Publisher<sensor_msgs::msg::TimeReference>::SharedPtr time_ref_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_raw_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_att_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr esf_ins_ros_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr fix_hp_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr imu_temp_pub_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr nav_diag_pub_;
+
+  // Republishers for U-Blox messages
   rclcpp::Publisher<ublox_msgs::msg::NavATT>::SharedPtr nav_att_pub_;
+  rclcpp::Publisher<ublox_msgs::msg::NavPVT>::SharedPtr nav_pvt_pub_;
+  rclcpp::Publisher<ublox_msgs::msg::NavHPPOSLLH>::SharedPtr nav_hpposllh_pub_;
+  rclcpp::Publisher<ublox_msgs::msg::NavHPPOSECEF>::SharedPtr nav_hpposecef_pub_;
   rclcpp::Publisher<ublox_msgs::msg::EsfINS>::SharedPtr esf_ins_pub_;
   rclcpp::Publisher<ublox_msgs::msg::EsfMEAS>::SharedPtr esf_meas_pub_;
   rclcpp::Publisher<ublox_msgs::msg::EsfRAW>::SharedPtr esf_raw_pub_;
@@ -85,9 +123,10 @@ class AdrUdrProduct final : public virtual ComponentInterface {
 
   std::string frame_id_;
   std::shared_ptr<diagnostic_updater::Updater> updater_;
+  bool use_highprecision_;
   rclcpp::Node* node_;
 };
 
 }  // namespace ublox_node
 
-#endif  // UBLOX_GPS_ADR_UDR_PRODUCT_HPP
+#endif  // UBLOX_GPS_ADR_UDR_PRODUCT_HPP

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};

ublox_gps/include/ublox_gps/utils.hpp

@@ -96,6 +96,7 @@ template <typename U>
 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<U>();