fix waitForSubscriber time

include/rviz_visual_tools/imarker_simple.hpp

@@ -73,7 +73,9 @@ class IMarkerSimple
 public:
   template <typename NodePtr>
   IMarkerSimple(NodePtr node, const std::string& imarker_topic_name = "imarker", double scale = 0.2,
-                const geometry_msgs::msg::Pose& initial_pose = getIdentityPose())
+                const geometry_msgs::msg::Pose& initial_pose = getIdentityPose(),
+                const rclcpp::QoS& update_pub_qos = rclcpp::QoS(1),
+                const rclcpp::QoS& feedback_sub_qos = rclcpp::QoS(1))
     : IMarkerSimple(node->get_node_base_interface(), node->get_node_clock_interface(),
                     node->get_node_logging_interface(), node->get_node_topics_interface(),
                     node->get_node_services_interface(), imarker_topic_name, scale, initial_pose)
@@ -86,7 +88,9 @@ class IMarkerSimple
                 const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr& topics_interface,
                 const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr& services_interface,
                 const std::string& imarker_topic_name = "imarker", double scale = 0.2,
-                const geometry_msgs::msg::Pose& initial_pose = getIdentityPose());
+                const geometry_msgs::msg::Pose& initial_pose = getIdentityPose(),
+                const rclcpp::QoS& update_pub_qos = rclcpp::QoS(1),
+                const rclcpp::QoS& feedback_sub_qos = rclcpp::QoS(1));
 
   geometry_msgs::msg::Pose& getPose();
 

include/rviz_visual_tools/rviz_visual_tools.hpp

@@ -245,27 +245,34 @@ class RvizVisualTools
   /**
    * \brief Load publishers as needed
    * \param wait_for_subscriber - whether a sleep for loop should be used to check for connectivity
-   * to an external node
-   *                              before proceeding
+   * to an external subscriber before proceeding
    */
-  void loadMarkerPub(bool wait_for_subscriber = false, bool latched = false);
+  void loadMarkerPub(bool wait_for_subscriber = false);
 
   /** \brief Optional blocking function to call *after* calling loadMarkerPub(). Allows you to do
-   * some intermediate
-   *         processing before wasting cycles waiting for the marker pub to find a subscriber
-   */
-  void waitForMarkerPub();
-  void waitForMarkerPub(double wait_time);
+   *         some intermediate processing before wasting cycles waiting for the marker pub to find a
+   * subscriber
+   * \param wait_time - Wait some amount of time for a subscriber before returning. Method will
+   * early exit if a subscriber is found before wait_time. Set to 0 to disable waiting and simply
+   * return if a subscriber exists.
+   * \return - true if a subscriber is found
+   */
+  bool waitForMarkerSub(double wait_time = 5);
+  [[deprecated("waitForMarkerPub deprecated. Use waitForMarkerSub instad")]] bool
+  waitForMarkerPub(double wait_time = 5)
+  {
+    return waitForMarkerSub(wait_time);
+  }
 
   /**
    * \brief Wait until at least one subscriber connects to a publisher
    * \param pub - the publisher to check for subscribers
-   * \param wait_time - time to wait for subscriber to be available before throwing warning
+   * \param wait_time - time to wait for subscriber to be available before throwing warning (sec)
    * \param blocking - if true, the function loop until a subscriber is gotten
    * \return true on successful connection
    */
   template <class PublisherPtr>
-  bool waitForSubscriber(const PublisherPtr& pub, double wait_time = 5, bool blocking = false);
+  bool waitForSubscriber(const PublisherPtr& pub, double wait_time = 5);
 
   /**
    * \brief Change the transparency of all markers published

launch/demo_combined.launch.py

@@ -41,16 +41,17 @@
 </launch>
 """
 
+
 def generate_launch_description():
     rviz_config = os.path.join(get_package_share_directory('rviz_visual_tools'),
                                'launch', 'demo.rviz')
 
     return LaunchDescription([
-        # ros2 run rviz2 rviz2 --display-config ./src/rviz_visual_tools/launch/demo.rviz
-        Node(package='rviz2', node_executable='rviz2', arguments=['--display-config', rviz_config], output='screen'),
-        Node(package='tf2_ros', node_executable='static_transform_publisher', arguments=["0", "0", "0", "0", "0", "0", "/world", "/base"], output='screen'),
+        Node(package='rviz2', node_executable='rviz2', arguments=[
+             '--display-config', rviz_config], output='screen'),
+        Node(package='tf2_ros', node_executable='static_transform_publisher', arguments=[
+             "0", "0", "0", "0", "0", "0", "/world", "/base"], output='screen'),
         Node(package='rviz_visual_tools', node_executable='rviz_visual_tools_demo', output='screen'),
-        Node(package='rviz_visual_tools', node_executable='rviz_visual_tools_imarker_simple_demo', output='screen')
+        Node(package='rviz_visual_tools',
+             node_executable='rviz_visual_tools_imarker_simple_demo', output='screen')
     ])
-
-

launch/demo_rviz.launch.py

@@ -32,13 +32,14 @@
 </launch>
 """
 
+
 def generate_launch_description():
     rviz_config = os.path.join(get_package_share_directory('rviz_visual_tools'),
                                'launch', 'demo.rviz')
 
     return LaunchDescription([
-        # ros2 run rviz2 rviz2 --display-config ./src/rviz_visual_tools/launch/demo.rviz
-        Node(package='rviz2', node_executable='rviz2', arguments=['--display-config', rviz_config], output='screen'),
-        Node(package='tf2_ros', node_executable='static_transform_publisher', arguments=["0", "0", "0", "0", "0", "0", "/world", "/base"], output='screen')
+        Node(package='rviz2', node_executable='rviz2', arguments=[
+             '--display-config', rviz_config], output='screen'),
+        Node(package='tf2_ros', node_executable='static_transform_publisher', arguments=[
+             "0", "0", "0", "0", "0", "0", "/world", "/base"], output='screen')
     ])
-

src/imarker_simple.cpp

@@ -54,7 +54,8 @@ IMarkerSimple::IMarkerSimple(
     const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr& topics_interface,
     const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr& services_interface,
     const std::string& imarker_topic_name, double scale,
-    const geometry_msgs::msg::Pose& initial_pose)
+    const geometry_msgs::msg::Pose& initial_pose, const rclcpp::QoS& update_pub_qos,
+    const rclcpp::QoS& feedback_sub_qos)
   : node_base_interface_(node_base_interface)
   , clock_interface_(clock_interface)
   , logging_interface_(logging_interface)
@@ -70,7 +71,7 @@ IMarkerSimple::IMarkerSimple(
 
   imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(
       imarker_topic, node_base_interface_, clock_interface_, logging_interface_, topics_interface_,
-      services_interface_);
+      services_interface_, update_pub_qos, feedback_sub_qos);
 
   // ros::Duration(2.0).sleep();
 

src/rviz_visual_tools.cpp

@@ -292,7 +292,7 @@ bool RvizVisualTools::loadRvizMarkers()
   return true;
 }
 
-void RvizVisualTools::loadMarkerPub(bool wait_for_subscriber, bool latched)
+void RvizVisualTools::loadMarkerPub(bool wait_for_subscriber)
 {
   if (pub_rviz_markers_ != nullptr)
   {
@@ -314,29 +314,23 @@ void RvizVisualTools::loadMarkerPub(bool wait_for_subscriber, bool latched)
   }
 }
 
-void RvizVisualTools::waitForMarkerPub()
+bool RvizVisualTools::waitForMarkerSub(double wait_time)
 {
-  bool blocking = true;
-  waitForSubscriber(pub_rviz_markers_, 0, blocking);
-}
-
-void RvizVisualTools::waitForMarkerPub(double wait_time)
-{
-  bool blocking = false;
-  waitForSubscriber(pub_rviz_markers_, wait_time, blocking);
+  return waitForSubscriber(pub_rviz_markers_, wait_time);
 }
 
 template <class PublisherPtr>
-bool RvizVisualTools::waitForSubscriber(const PublisherPtr& pub, double wait_time, bool blocking)
+bool RvizVisualTools::waitForSubscriber(const PublisherPtr& pub, double wait_time)
 {
   // Will wait at most this amount of time
-  rclcpp::Time max_time(clock_interface_->get_clock()->now() + rclcpp::Duration(wait_time));
+  rclcpp::Time max_time(clock_interface_->get_clock()->now() +
+                        rclcpp::Duration::from_seconds(wait_time));
 
   // This is wrong. It returns only the number of subscribers that have already
   // established their direct connections to this publisher
 
   // How often to check for subscribers
-  rclcpp::Duration loop_duration(1.0 / 200.0);
+  rclcpp::Duration loop_duration = rclcpp::Duration::from_seconds(1.0 / 200.0);
   if (!pub)
   {
     RCLCPP_ERROR(logger_,
@@ -345,40 +339,39 @@ bool RvizVisualTools::waitForSubscriber(const PublisherPtr& pub, double wait_tim
 
   std::string topic_name = pub->get_topic_name();
   int num_existing_subscribers = graph_interface_->count_subscribers(topic_name);
-  if (blocking && num_existing_subscribers == 0)
+  if (wait_time > 0 && num_existing_subscribers == 0)
   {
-    std::stringstream ss;
-    ss << "Topic '" << pub->get_topic_name() << "' waiting for subscriber...";
-    RCLCPP_INFO(logger_, ss.str().c_str());
+    RCLCPP_INFO(logger_, "Topic '%s' waiting %f seconds for subscriber, ", pub->get_topic_name(),
+                 wait_time);
   }
 
   // Wait for subscriber
-  while (num_existing_subscribers == 0 && rclcpp::ok())
+  while (wait_time > 0 && num_existing_subscribers == 0 && rclcpp::ok())
   {
-    if (!blocking && clock_interface_->get_clock()->now() > max_time)  // Check if timed out
+    if (clock_interface_->get_clock()->now() > max_time)  // Check if timed out
     {
-      std::stringstream ss;
-      ss << "Topic '" << pub->get_topic_name() << "' unable to connect to any subscribers within "
-         << wait_time << " sec. It is possible initially published visual messages "
-                         "will be lost.";
-      RCLCPP_WARN(logger_, ss.str().c_str());
-      return false;
+      RCLCPP_WARN(logger_, "Topic '%s' unable to connect to any subscribers within %f sec. It is "
+                            "possible initially published visual messages will be lost.",
+                   pub->get_topic_name(), wait_time);
+      pub_rviz_markers_connected_ = false;
+      return pub_rviz_markers_connected_;
     }
 
-    // Sleep
-    rclcpp::sleep_for(std::chrono::nanoseconds(loop_duration.nanoseconds()));
-
     // Check again
     num_existing_subscribers = graph_interface_->count_subscribers(topic_name);
+
+    // Sleep
+    rclcpp::sleep_for(std::chrono::nanoseconds(loop_duration.nanoseconds()));
   }
 
   if (!rclcpp::ok())
   {
+    pub_rviz_markers_connected_ = false;
     return false;
   }
-  pub_rviz_markers_connected_ = true;
 
-  return true;
+  pub_rviz_markers_connected_ = (num_existing_subscribers != 0);
+  return pub_rviz_markers_connected_;
 }
 
 void RvizVisualTools::setLifetime(double lifetime)

src/rviz_visual_tools_demo.cpp

@@ -72,10 +72,13 @@ class RvizVisualToolsDemo : public rclcpp::Node
   {
     visual_tools_.reset(
         new rvt::RvizVisualTools("world", "/rviz_visual_tools", dynamic_cast<rclcpp::Node*>(this)));
-    visual_tools_->loadMarkerPub();  // create publisher before waiting
-
-    RCLCPP_INFO(get_logger(), "Sleeping 2 seconds before running demo");
-    rclcpp::sleep_for(2s);
+    // create publisher before waiting
+    visual_tools_->loadMarkerPub();
+    bool has_sub = visual_tools_->waitForMarkerSub(10.0);
+    if (!has_sub)
+      RCLCPP_INFO(
+          get_logger(),
+          "/rviz_visual_tools does not have a subscriber after 10s. Visualizations may be lost");
 
     // Clear messages
     visual_tools_->deleteAllMarkers();