From bd9cf1847c073c7c97ee1bffe629e717ba5cc10a Mon Sep 17 00:00:00 2001
From: Mike O'Driscoll <modriscoll@clearpathrobotics.com>
Date: Thu, 9 Nov 2017 15:54:20 -0500
Subject: [PATCH] Add overall spin timeout to rosserial read.

Reading may block for to long, while there is a check
for a timeout, it is not at the global scope of the while(true)
statement.
---
 .../src/ros_lib/ros/node_handle.h             | 43 +++++++++++++++++--
 1 file changed, 39 insertions(+), 4 deletions(-)

diff --git a/rosserial_client/src/ros_lib/ros/node_handle.h b/rosserial_client/src/ros_lib/ros/node_handle.h
index 4c8f6042c..67d990357 100644
--- a/rosserial_client/src/ros_lib/ros/node_handle.h
+++ b/rosserial_client/src/ros_lib/ros/node_handle.h
@@ -64,6 +64,10 @@ class NodeHandleBase_
 namespace ros
 {
 
+const int SPIN_OK = 0;
+const int SPIN_ERR = -1;
+const int SPIN_TIMEOUT = -2;
+
 const uint8_t SYNC_SECONDS  = 5;
 const uint8_t MODE_FIRST_FF = 0;
 /*
@@ -108,6 +112,9 @@ class NodeHandle_ : public NodeHandleBase_
   /* used for computing current time */
   uint32_t sec_offset, nsec_offset;
 
+  /* Spinonce maximum work timeout */
+  uint32_t spin_timeout_;
+
   uint8_t message_in[INPUT_SIZE];
   uint8_t message_out[OUTPUT_SIZE];
 
@@ -139,6 +146,8 @@ class NodeHandle_ : public NodeHandleBase_
     req_param_resp.floats = NULL;
     req_param_resp.ints_length = 0;
     req_param_resp.ints = NULL;
+
+    spin_timeout_ = 0;
   }
 
   Hardware* getHardware()
@@ -166,6 +175,19 @@ class NodeHandle_ : public NodeHandleBase_
     topic_ = 0;
   };
 
+  /**
+   * @brief Sets the maximum time in millisconds that spinOnce() can work.
+   * This will not effect the processing of the buffer, as spinOnce processes
+   * one byte at a time. It simply sets the maximum time that one call can
+   * process for. You can choose to clear the buffer if that is beneficial if
+   * SPIN_TIMEOUT is returned from spinOnce().
+   * @param timeout The timeout in milliseconds that spinOnce will function.
+   */
+  void setSpinTimeout(const uint32_t& timeout)
+  {
+     spin_timeout_ = timeout;
+  }
+
 protected:
   //State machine variables for spinOnce
   int mode_;
@@ -189,7 +211,6 @@ class NodeHandle_ : public NodeHandleBase_
 
   virtual int spinOnce()
   {
-
     /* restart if timed out */
     uint32_t c_time = hardware_.time();
     if ((c_time - last_sync_receive_time) > (SYNC_SECONDS * 2200))
@@ -209,6 +230,20 @@ class NodeHandle_ : public NodeHandleBase_
     /* while available buffer, read data */
     while (true)
     {
+      // If a timeout has been specified, check how long spinOnce has been running.
+      if (spin_timeout_ > 0)
+      {
+        // If the maximum processing timeout has been exceeded, exit with error.
+        // The next spinOnce can continue off where it left off, or optionally
+        // based on the application in use, the hardware buffer could be flushed
+        // and start fresh.
+        if ((hardware_.time() - c_time) > spin_timeout_)
+        {
+          // Reset back to first mode.
+          mode_ = MODE_FIRST_FF;
+          return SPIN_TIMEOUT;
+        }
+      }
       int data = hardware_.read();
       if (data < 0)
         break;
@@ -231,7 +266,7 @@ class NodeHandle_ : public NodeHandleBase_
         {
           /* We have been stuck in spinOnce too long, return error */
           configured_ = false;
-          return -2;
+          return SPIN_TIMEOUT;
         }
       }
       else if (mode_ == MODE_PROTOCOL_VER)
@@ -290,7 +325,7 @@ class NodeHandle_ : public NodeHandleBase_
             negotiateTopics();
             last_sync_time = c_time;
             last_sync_receive_time = c_time;
-            return -1;
+            return SPIN_ERR;
           }
           else if (topic_ == TopicInfo::ID_TIME)
           {
@@ -321,7 +356,7 @@ class NodeHandle_ : public NodeHandleBase_
       last_sync_time = c_time;
     }
 
-    return 0;
+    return SPIN_OK;
   }