Fix ros1 topic discovery function not being called frequently at startup

foxglove · Nov 23, 2022 · 7f0f88e · 7f0f88e
1 parent d47162d
commit 7f0f88e
Showing 1 changed file with 7 additions and 5 deletions.
diff --git a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp
@@ -23,6 +23,7 @@ constexpr char DEFAULT_ADDRESS[] = "0.0.0.0";
 constexpr int DEFAULT_MAX_UPDATE_MS = 5000;
 constexpr char ROS1_CHANNEL_ENCODING[] = "ros1";
 constexpr uint32_t SUBSCRIPTION_QUEUE_LENGTH = 10;
+constexpr double MIN_UPDATE_PERIOD_MS = 100.0;
 
 using TopicAndDatatype = std::pair<std::string, std::string>;
 using SubscriptionsByClient = std::map<foxglove::ConnHandle, ros::Subscriber, std::owner_less<>>;
@@ -34,10 +35,10 @@ class FoxgloveBridge : public nodelet::Nodelet {
     auto& nhp = getPrivateNodeHandle();
     const auto address = nhp.param<std::string>("address", DEFAULT_ADDRESS);
     const int port = nhp.param<int>("port", DEFAULT_PORT);
-    const int max_update_ms = nhp.param<int>("max_update_ms", DEFAULT_MAX_UPDATE_MS);
     const auto useTLS = nhp.param<bool>("tls", false);
     const auto certfile = nhp.param<std::string>("certfile", "");
     const auto keyfile = nhp.param<std::string>("keyfile", "");
+    _maxUpdateMs = static_cast<size_t>(nhp.param<int>("max_update_ms", DEFAULT_MAX_UPDATE_MS));
 
     ROS_INFO("Starting %s with %s", ros::this_node::getName().c_str(),
              foxglove::WebSocketUserAgent());
@@ -59,8 +60,7 @@ class FoxgloveBridge : public nodelet::Nodelet {
                                                std::placeholders::_1, std::placeholders::_2));
       _server->start(address, static_cast<uint16_t>(port));
 
-      _updateTimer = getMTNodeHandle().createTimer(ros::Duration(max_update_ms / 1e3),
-                                                   &FoxgloveBridge::updateAdvertisedTopics, this);
+      updateAdvertisedTopics(ros::TimerEvent());
     } catch (const std::exception& err) {
       ROS_ERROR("Failed to start websocket server: %s", err.what());
       // Rethrow exception such that the nodelet is unloaded.
@@ -267,9 +267,11 @@ class FoxgloveBridge : public nodelet::Nodelet {
       _server->broadcastChannels();
     }
 
-    // Schedule the next update using truncated exponential backoff, up to `_maxUpdateMs`
+    // Schedule the next update using truncated exponential backoff, between `MIN_UPDATE_PERIOD_MS`
+    // and `_maxUpdateMs`
     _updateCount++;
-    auto nextUpdateMs = static_cast<double>(std::min(size_t(1) << _updateCount, _maxUpdateMs));
+    const auto nextUpdateMs = std::max(
+      MIN_UPDATE_PERIOD_MS, static_cast<double>(std::min(size_t(1) << _updateCount, _maxUpdateMs)));
     _updateTimer = getMTNodeHandle().createTimer(ros::Duration(nextUpdateMs / 1e3),
                                                  &FoxgloveBridge::updateAdvertisedTopics, this);
   }