From ccd1bcf98f8f85981ec9d369c27042a23a4b9a73 Mon Sep 17 00:00:00 2001
From: XU Hao <xuhao3e8@gmail.com>
Date: Sun, 17 Mar 2019 09:30:27 +0000
Subject: [PATCH] Reduce CPU laod fromm 100% to 4.9%

---
 tfmini_ros/include/TFmini.h        |  2 +-
 tfmini_ros/src/TFmini.cpp          | 12 ++++++++++--
 tfmini_ros/src/TFmini_ros_node.cpp |  7 +++++--
 3 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/tfmini_ros/include/TFmini.h b/tfmini_ros/include/TFmini.h
index 11fd918..e298f51 100644
--- a/tfmini_ros/include/TFmini.h
+++ b/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);
   };
 }
diff --git a/tfmini_ros/src/TFmini.cpp b/tfmini_ros/src/TFmini.cpp
index e4a3ff6..2798e8d 100644
--- a/tfmini_ros/src/TFmini.cpp
+++ b/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)
diff --git a/tfmini_ros/src/TFmini_ros_node.cpp b/tfmini_ros/src/TFmini_ros_node.cpp
index 93a82e1..575819a 100644
--- a/tfmini_ros/src/TFmini_ros_node.cpp
+++ b/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();