Skip publishing a topic with no subscribers (#12)

Co-authored-by: Hiroaki Masuzawa <[email protected]>
choreonoid · Jun 26, 2023 · cc3d8a1 · cc3d8a1
1 parent 28ec820
commit cc3d8a1
Showing 1 changed file with 27 additions and 0 deletions.
diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp
@@ -369,6 +369,9 @@ void BodyROSItem::updateForceSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     geometry_msgs::WrenchStamped force;
     force.header.stamp.fromSec(io->currentTime());
     force.header.frame_id = sensor->name();
@@ -388,6 +391,9 @@ void BodyROSItem::updateRateGyroSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::Imu gyro;
     gyro.header.stamp.fromSec(io->currentTime());
     gyro.header.frame_id = sensor->name();
@@ -404,6 +410,9 @@ void BodyROSItem::updateAccelSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::Imu accel;
     accel.header.stamp.fromSec(io->currentTime());
     accel.header.frame_id = sensor->name();
@@ -420,6 +429,9 @@ void BodyROSItem::updateImu
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::Imu imu;
     imu.header.stamp.fromSec(io->currentTime());
     imu.header.frame_id = sensor->name();
@@ -439,6 +451,9 @@ void BodyROSItem::updateVisionSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::Image vision;
     vision.header.stamp.fromSec(io->currentTime());
     vision.header.frame_id = sensor->name();
@@ -465,6 +480,9 @@ void BodyROSItem::updateRangeVisionSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::PointCloud2 range;
     range.header.stamp.fromSec(io->currentTime());
     range.header.frame_id = sensor->name();
@@ -539,6 +557,9 @@ void BodyROSItem::updateRangeSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::LaserScan range;
     range.header.stamp.fromSec(io->currentTime());
     range.header.frame_id = sensor->name();
@@ -571,6 +592,9 @@ void BodyROSItem::update3DRangeSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::PointCloud2 range;
     // Header Info
     range.header.stamp.fromSec(io->currentTime());
@@ -645,6 +669,9 @@ void BodyROSItem::update3DRangeSensor
     if(!sensor->on()){
         return;
     }
+    if(publisher.getNumSubscribers() == 0){
+        return;
+    }
     sensor_msgs::PointCloud range;
     // Header Info
     range.header.stamp.fromSec(io->currentTime());