robopeak#50

PINTO0309 · May 2, 2018 · 2152d43 · 2152d43
1 parent b7d80bc
commit 2152d43
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/src/node.cpp b/src/node.cpp
@@ -242,6 +242,7 @@ int main(int argc, char * argv[]) {
     ros::Time start_scan_time;
     ros::Time end_scan_time;
     double scan_duration;
+    uint32_t prev_num_subscribers=(uint32_t)-1;
     while (ros::ok()) {
 
         rplidar_response_measurement_node_t nodes[360*2];
@@ -310,6 +311,18 @@ int main(int argc, char * argv[]) {
             }
         }
 
+        uint32_t num_subcribers = scan_pub.getNumSubscribers();
+        if (num_subcribers == 0 && prev_num_subscribers != 0) {
+            ROS_DEBUG("No subscribers: Stop motor");
+            drv->stop();
+            drv->stopMotor();
+        } else if (num_subcribers != 0 && prev_num_subscribers == 0) {
+            ROS_DEBUG("At least one subscribers: Start motor");
+            drv->startMotor(motor_pwm);
+            drv->startScan();
+        }
+        prev_num_subscribers = num_subcribers;
+
         ros::spinOnce();
     }