Always publish debug messages (#493)

bit-bots · Jun 20, 2024 · 1ff5a2b · 1ff5a2b
2 parents bfb9a40 + f8c298a
commit 1ff5a2b
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
@@ -185,13 +185,12 @@ void WalkNode::run() {
           pub_support_->publish(support_state);
           current_support_foot_ = support_state.phase;
         }
-
-        // publish debug information
-        if (config_.node.debug_active) {
-          publish_debug();
-        }
       }
     }
+    // publish debug information
+    if (config_.node.debug_active) {
+      publish_debug();
+    }
     // always publish odometry to not confuse odometry fuser
     pub_odometry_->publish(getOdometry());
   }

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp
@@ -329,6 +329,10 @@ void WalkVisualizer::publishArrowMarker(std::string name_space, std::string fram
 }
 
 void WalkVisualizer::publishWalkMarkers(WalkResponse response) {
+  // only do something if someone is listing
+  if (pub_debug_marker_->get_subscription_count() == 0) {
+    return;
+  }
   // publish markers
   visualization_msgs::msg::Marker marker_msg;
   marker_msg.header.stamp = node_->now();