diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp index 011caaf..f77f238 100644 --- 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());