From f5c32dd1c38ecefab628cfce9b02366c56254913 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 1 Nov 2020 15:41:55 -0500
Subject: [PATCH] voxel_markers: making subscription lazy

---
 src/costmap_2d/voxel_markers.cpp | 20 ++++++++++++++++++--
 1 file changed, 18 insertions(+), 2 deletions(-)

diff --git a/src/costmap_2d/voxel_markers.cpp b/src/costmap_2d/voxel_markers.cpp
index 1f93a887..405f7e3d 100644
--- a/src/costmap_2d/voxel_markers.cpp
+++ b/src/costmap_2d/voxel_markers.cpp
@@ -144,6 +144,20 @@ void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPt
   ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec());
 }
 
+ros::Publisher pub;
+ros::Subscriber sub;
+void connectCb()
+{
+	ros::NodeHandle n;
+	sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
+}
+
+void disconnectCb()
+{
+	if(pub.getNumSubscribers()==0)
+		sub.shutdown();
+}
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "3d_markers");
@@ -158,8 +172,10 @@ int main(int argc, char** argv)
 
   ROS_DEBUG("Startup");
 
-  ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1);
-  ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
+  ros::SubscriberStatusCallback connect_cb = boost::bind(connectCb);
+  ros::SubscriberStatusCallback disconnect_cb = boost::bind(disconnectCb);
+
+  pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1, connect_cb, disconnect_cb);
   g_marker_ns = n.resolveName("voxel_grid");
 
   ros::spin();