Reduce CPU load from 100% to 4.9%

tfmini_ros/include/TFmini.h

@@ -26,7 +26,7 @@ namespace benewake
       std::string portName_;
       int baudRate_;
       int serial_;
-
+      int available_bytes();
       bool readData(unsigned char *_buf, int _nRead);
   };
 }

tfmini_ros/src/TFmini.cpp

@@ -1,6 +1,6 @@
 #include <TFmini.h>
 #include <ros/ros.h>
-
+#include <sys/ioctl.h>
 //#define DEBUG
 
 namespace benewake
@@ -85,7 +85,9 @@ namespace benewake
     bool TFmini::readData(unsigned char* _buf, int _nRead)
     {
       int total = 0, ret = 0;
-
+      while (available_bytes() < _nRead) {
+        usleep(100);
+      }
       while(total != _nRead)
       {
         ret = read(serial_, _buf + total, (_nRead - total));
@@ -106,6 +108,12 @@ namespace benewake
       return true;
     }
 
+    int TFmini::available_bytes() {
+      int ret = 0;
+      ioctl(serial_, FIONREAD, &ret);
+      return ret;
+    }
+
     float TFmini::getDist()
     {
       while(true)

tfmini_ros/src/TFmini_ros_node.cpp

@@ -9,6 +9,8 @@ int main(int argc, char **argv)
   int baud_rate;
   benewake::TFmini *tfmini_obj;
 
+  ros::Rate r(60);
+
   nh.param("serial_port", portName, std::string("/dev/ttyUSB0"));
   nh.param("baud_rate", baud_rate, 115200);
 
@@ -23,9 +25,9 @@ int main(int argc, char **argv)
   float dist = 0;
   ROS_INFO_STREAM("Start processing ...");
 
-  while(ros::master::check() && ros::ok())
+  while( ros::ok())
   {
-    ros::spinOnce();
+    // ros::spinOnce();
     dist = tfmini_obj->getDist();
     if(dist > 0 && dist < TFmini_range.max_range)
     {
@@ -42,6 +44,7 @@ int main(int argc, char **argv)
     {
       ROS_ERROR_STREAM("Data validation error!");
     }
+    // r.sleep();
   }
 
   tfmini_obj->closePort();