Dashing: Don't wait for acknowledge in setTimeMode.

ublox_gps/config/c94_m8p_rover.yaml

@@ -23,7 +23,7 @@ ublox_gps_node:
     dr_limit: 0
 
     # TMODE3 Config
-    tmode3: 0                   # Survey-In Mode
+    tmode3: 0                   # Disabled
     sv_in:
       reset: false              # True: disables and re-enables survey-in (resets)
                                 # False: Disables survey-in only if TMODE3 is

ublox_gps/include/ublox_gps/gps.hpp

@@ -191,18 +191,19 @@ class Gps final {
    * @brief Configure the device navigation and measurement rate settings.
    * @param meas_rate Period in milliseconds between subsequent measurements.
    * @param nav_rate the rate at which navigation solutions are generated by the
+   * @param wait Whether to wait for acknowledgement
    * receiver in number measurement cycles
    * @return true on ACK, false on other conditions.
    */
-  bool configRate(uint16_t meas_rate, uint16_t nav_rate);
+  bool configRate(uint16_t meas_rate, uint16_t nav_rate, bool wait);
 
   /**
    * @brief Configure the RTCM messages with the given IDs to the set rate.
-   * @param ids the RTCM message ids, valid range: [0, 255]
-   * @param rates the send rates for each RTCM message ID, valid range: [0, 255]
+   * @param rtcms the RTCM message structures containing ids and rates
+   * @param wait Whether to wait for acknowledgement
    * @return true on ACK, false on other conditions.
    */
-  bool configRtcm(const std::vector<Rtcm> & rtcms);
+  bool configRtcm(const std::vector<Rtcm> & rtcms, bool wait);
 
   /**
    * @brief Configure the SBAS settings.
@@ -253,9 +254,10 @@ class Gps final {
    * @param class_id the class identifier of the message
    * @param message_id the message identifier
    * @param rate the updated rate in Hz
+   * @param wait Whether to wait for acknowledgement
    * @return true on ACK, false on other conditions.
    */
-  bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate);
+  bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate, bool wait);
 
   /**
    * @brief Set the device dynamic model.
@@ -493,7 +495,7 @@ class Gps final {
 template <typename T>
 void Gps::subscribe(
     typename CallbackHandler_<T>::Callback callback, unsigned int rate) {
-  if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) {
+  if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate, true)) {
     return;
   }
   subscribe<T>(callback);

ublox_gps/src/gps.cpp

@@ -270,7 +270,7 @@ void Gps::reset(const std::chrono::milliseconds& wait) {
   configured_ = false;
   // sleep because of undefined behavior after I/O reset
   std::this_thread::sleep_for(wait);
-  if (host_ == "") {
+  if (host_.empty()) {
     resetSerial(port_);
   } else {
     initializeTcp(host_, port_);
@@ -392,21 +392,26 @@ bool Gps::configUsb(uint16_t tx_ready,
   return configure(port);
 }
 
-bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate) {
+bool Gps::configRate(uint16_t meas_rate, uint16_t nav_rate, bool wait) {
   RCLCPP_DEBUG(logger_, "Configuring measurement rate to %u ms and nav rate to %u cycles",
                meas_rate, nav_rate);
 
   ublox_msgs::msg::CfgRATE rate;
   rate.meas_rate = meas_rate;
   rate.nav_rate = nav_rate;  //  must be fixed at 1 for ublox 5 and 6
   rate.time_ref = ublox_msgs::msg::CfgRATE::TIME_REF_GPS;
-  return configure(rate);
+  return configure(rate, wait);
 }
 
-bool Gps::configRtcm(const std::vector<Rtcm> & rtcms) {
+bool Gps::configRtcm(const std::vector<Rtcm> & rtcms, bool wait) {
   for (const Rtcm & rtcm : rtcms) {
     RCLCPP_DEBUG(logger_, "Setting RTCM %d Rate %u", rtcm.id, rtcm.rate);
-    if (!setRate(ublox_msgs::Class::RTCM, rtcm.id, rtcm.rate)) {
+
+    ublox_msgs::msg::CfgMSGS msgs;
+    msgs.msg_class = ublox_msgs::Class::RTCM;
+    msgs.msg_id = rtcm.id;
+    msgs.rates[ublox_msgs::msg::CfgMSGS::PORT_ID_UART1] = rtcm.rate;
+    if (!configure(msgs, wait)) {
       RCLCPP_ERROR(logger_, "Could not set RTCM %d to rate %u", rtcm.id, rtcm.rate);
       return false;
     }
@@ -479,14 +484,14 @@ bool Gps::disableTmode3() {
   return configure(tmode3);
 }
 
-bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate) {
+bool Gps::setRate(uint8_t class_id, uint8_t message_id, uint8_t rate, bool wait) {
   RCLCPP_DEBUG_EXPRESSION(logger_, debug_ >= 2, "Setting rate 0x%02x, 0x%02x, %u", class_id,
                           message_id, rate);
   ublox_msgs::msg::CfgMSG msg;
   msg.msg_class = class_id;
   msg.msg_id = message_id;
   msg.rate = rate;
-  return configure(msg);
+  return configure(msg, wait);
 }
 
 bool Gps::setDynamicModel(uint8_t model) {

ublox_gps/src/hpg_ref_product.cpp

@@ -109,7 +109,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr<ublox_gps::Gps> gps) {
                                fixed_pos_acc_)) {
       throw std::runtime_error("Failed to set TMODE3 to fixed.");
     }
-    if (!gps->configRtcm(rtcms_)) {
+    if (!gps->configRtcm(rtcms_, true)) {
       throw std::runtime_error("Failed to set RTCM rates");
     }
     mode_ = FIXED;
@@ -150,7 +150,7 @@ bool HpgRefProduct::configureUblox(std::shared_ptr<ublox_gps::Gps> gps) {
       meas_rate_temp = kDefaultMeasPeriod;
     }
     // Set nav rate to 1 Hz during survey in
-    if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp)) {
+    if (!gps->configRate(meas_rate_temp, 1000 / meas_rate_temp, true)) {
       throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") +
                                "before setting TMODE3 to survey-in.");
     }
@@ -197,12 +197,19 @@ bool HpgRefProduct::setTimeMode(std::shared_ptr<ublox_gps::Gps> gps) {
 
   // Set the Measurement & nav rate to user config
   // (survey-in sets nav_rate to 1 Hz regardless of user setting)
-  if (!gps->configRate(meas_rate_, nav_rate_)) {
+
+  // setTimeMode is called during a GPS read callback.  Because of that,
+  // we *cannot* wait for acknowledgement down in the GPS layer; it would
+  // result in deadlock.  Instead we set the rate without waiting and hope
+  // for the best.
+  if (!gps->configRate(meas_rate_, nav_rate_, false)) {
     RCLCPP_ERROR(node_->get_logger(), "Failed to set measurement rate to %d ms navigation rate to %d cycles",
                  meas_rate_, nav_rate_);
   }
   // Enable the RTCM out messages
-  if (!gps->configRtcm(rtcms_)) {
+  // We cannot wait for acknowledgement that this completed for the same reason
+  // as above.
+  if (!gps->configRtcm(rtcms_, false)) {
     RCLCPP_ERROR(node_->get_logger(), "Failed to configure RTCM IDs");
     return false;
   }

ublox_gps/src/node.cpp

@@ -701,7 +701,7 @@ bool UbloxNode::configureUblox() {
       if (set_usb_) {
         gps_->configUsb(usb_tx_, usb_in_, usb_out_);
       }
-      if (!gps_->configRate(meas_rate_, nav_rate_)) {
+      if (!gps_->configRate(meas_rate_, nav_rate_, true)) {
         std::stringstream ss;
         ss << "Failed to set measurement rate to " << meas_rate_
           << "ms and navigation rate to " << nav_rate_;

ublox_msgs/CMakeLists.txt

@@ -31,6 +31,7 @@ set(msg_files
   "msg/CfgINFBlock.msg"
   "msg/CfgINF.msg"
   "msg/CfgMSG.msg"
+  "msg/CfgMSGS.msg"
   "msg/CfgNAV5.msg"
   "msg/CfgNAVX5.msg"
   "msg/CfgNMEA6.msg"

ublox_msgs/include/ublox_msgs/serialization.hpp

@@ -458,6 +458,40 @@ struct UbloxSerializer<ublox_msgs::msg::CfgMSG_<ContainerAllocator> > {
   }
 };
 
+template <typename ContainerAllocator>
+struct UbloxSerializer<ublox_msgs::msg::CfgMSGS_<ContainerAllocator> > {
+  inline static void read(const uint8_t *data, uint32_t count,
+                          ublox_msgs::msg::CfgMSGS_<ContainerAllocator> &m) {
+    UbloxIStream stream(const_cast<uint8_t *>(data), count);
+    stream.next(m.msg_class);
+    stream.next(m.msg_id);
+    stream.next(m.rates[0]);
+    stream.next(m.rates[1]);
+    stream.next(m.rates[2]);
+    stream.next(m.rates[3]);
+    stream.next(m.rates[4]);
+    stream.next(m.rates[5]);
+  }
+
+  inline static uint32_t serializedLength(const ublox_msgs::msg::CfgMSGS_<ContainerAllocator> &m) {
+    (void)m;
+    return 8;
+  }
+
+  inline static void write(uint8_t *data, uint32_t size,
+                           const ublox_msgs::msg::CfgMSGS_<ContainerAllocator> &m) {
+    UbloxOStream stream(data, size);
+    stream.next(m.msg_class);
+    stream.next(m.msg_id);
+    stream.next(m.rates[0]);
+    stream.next(m.rates[1]);
+    stream.next(m.rates[2]);
+    stream.next(m.rates[3]);
+    stream.next(m.rates[4]);
+    stream.next(m.rates[5]);
+  }
+};
+
 template <typename ContainerAllocator>
 struct UbloxSerializer<ublox_msgs::msg::CfgNAV5_<ContainerAllocator> > {
   inline static void read(const uint8_t *data, uint32_t count,

ublox_msgs/include/ublox_msgs/ublox_msgs.hpp

@@ -73,6 +73,7 @@
 #include <ublox_msgs/msg/cfg_inf.hpp>
 #include <ublox_msgs/msg/cfg_inf_block.hpp>
 #include <ublox_msgs/msg/cfg_msg.hpp>
+#include <ublox_msgs/msg/cfg_msgs.hpp>
 #include <ublox_msgs/msg/cfg_nav5.hpp>
 #include <ublox_msgs/msg/cfg_navx5.hpp>
 #include <ublox_msgs/msg/cfg_nmea.hpp>

ublox_msgs/msg/CfgMSG.msg

@@ -1,5 +1,5 @@
 # CFG-MSG (0x06 0x01)
-# Message Rate(s)
+# Message Rate
 #
 # Set message rate for the current port
 

ublox_msgs/msg/CfgMSGS.msg

@@ -0,0 +1,16 @@
+# CFG-MSG (0x06 0x01)
+# Message Rates
+#
+# Set message rate for all ports - confusingly has the same name, class, and message ID as CfgMSG, but allows to setup all ports
+
+uint8 CLASS_ID = 6
+uint8 MESSAGE_ID = 1
+
+uint8 msg_class            # Message Class
+uint8 msg_id               # Message Identifier
+uint8[6] rates             # The rate for each of the ports
+uint8 PORT_ID_DDC = 0
+uint8 PORT_ID_UART1 = 1
+uint8 PORT_ID_UART2 = 2
+uint8 PORT_ID_USB = 3
+uint8 PORT_ID_SPI = 4