Merge pull request #1 from ikh-innovation/nothread

Nothread

config/io.yaml

@@ -4,49 +4,41 @@ numato:
       pin: 12
       topic_name: "charging_limit_switch"
       logic: "Normal"
-      rate: 5.0
     rain_sensor:
       pin: 7
       topic_name: "rain_sensor"
       logic: "Inverted"
-      rate: 2.0
 
   outputs:
     mowing_led_indicator: 
       pin: 30
       topic_name: "mowing_led_indicator"
       logic: "Normal"
-      rate: 2.0
     buzzer: 
       pin: 31
       topic_name: "buzzer"
       logic: "Normal"
-      rate: 2.0
 
   constants:
     rain_pull_up: 
       pin: 6
       topic_name: "rain_pull_up"
       logic: "Normal"
-      rate: 1.0
       value: true
     rear_bumper_pull_up: 
       pin: 15
       topic_name: "rear_bumper_pull_up"
       logic: "Normal"
-      rate: 1.0
       value: true
     front_bumper_pull_up: 
       pin: 23
       topic_name: "front_bumper_pull_up"
       logic: "Normal"
-      rate: 1.0
       value: true
 
   bumper_v2:
     bumper:
       topic_name: "bumper"
-      rate: 10.0
       logic: "Inverted"
       low_center_bumper: 17
       low_right_bumper: 18
@@ -57,7 +49,6 @@ numato:
       state_bumper: 22
     rear_bumper:
       topic_name: "rear_bumper"
-      rate: 10.0
       logic: "Inverted"
       low_right_bumper: 14
       low_left_bumper: 13

scripts/numato_ros

@@ -8,7 +8,7 @@ import sys
 from ikh_ros_msgs.msg import Labjack_dout, Bumper_v2
 from time import sleep
 from std_msgs.msg import Bool
-from threading import Lock
+
 
 CMDS = {
     0: "clear",
@@ -17,7 +17,7 @@ CMDS = {
     False: "clear"
 }
 
-DEBUG = False
+
 
 class GPIO:
     def __init__(self,serPort,logic,SERIAL_READ_SIZE=25):
@@ -33,54 +33,102 @@ class GPIO:
 
     def readIO(self,pin):
         try:
-            # tic = rospy.Time.now().to_sec()
             self.serPort.write("gpio read "+ self.pin_to_index(pin) + "\n\r")
             res  = str(self.serPort.read(self.SERIAL_READ_SIZE))
-            # print("> ReadIO at: {}".format(rospy.Time.now().to_sec()-tic))
             return res[-4]
         except Exception as e: 
                 rospy.logwarn("Cannot read serial: {}".format(e))
                 return False
 
+
+    def getAvgBool(self,arr,pin=None):
+        true_counts = arr.count(True)
+        false_counts = arr.count(False)
+        res_size = len(arr)
+        # print("> readIOAvg at: {}".format(rospy.Time.now().to_sec()-tic))
+        # Check if all inputs are the same
+        if (arr.count(False)==len(arr)):
+            return False
+        elif (arr.count(True)==len(arr)):
+            return True
+        else:
+            if(pin!=None):
+                rospy.logwarn("PIN: {}: False trigger IO, return False | True counts: {} False counts: {} Result size: {}".format(pin,true_counts,false_counts,res_size))
+            else:
+                rospy.logwarn("False trigger IO, return False | True counts: {} False counts: {} Result size: {}".format(true_counts,false_counts,res_size))
+            return False
+
+
+
     def readIOAvgBool(self,pin,readtimes=8):
         results = []
         try:
-            # tic = rospy.Time.now().to_sec()
             for i in range(readtimes):
                 self.serPort.write("gpio read "+ self.pin_to_index(pin) + "\n\r")
                 res  = str(self.serPort.read(self.SERIAL_READ_SIZE))
                 results.append(self.str2bool(res[-4]))
-                # rospy.sleep(0.001)
-
-            true_counts = results.count(True)
-            false_counts = results.count(False)
-            res_size = len(results)
-            # print("> readIOAvg at: {}".format(rospy.Time.now().to_sec()-tic))
-            # Check if all inputs are the same
-            if (results.count(False)==len(results)):
-                return False
-            elif (results.count(True)==len(results)):
-                return True
-            else:
-                rospy.logwarn("PIN: {}: False trigger IO, return False | True counts: {} False counts: {} Result size: {}".format(pin,true_counts,false_counts,res_size))
-                return False
+            return self.getAvgBool(results,pin)
+
         except Exception as e: 
                 rospy.logwarn("Cannot read serial: {}".format(e))
                 return False
 
     def readMultIO(self,pins=[]):
-        self.serPort.write("gpio readall\n\r")
-        res  = str(self.serPort.read(1000))
-        print(res)
-        # print("response ({len(res)} byte):\n{res}")
-        print(" ".join(hex(c) for c in res))
-
-
+        try:
+            self.serPort.write("gpio readall\n\r")
+            res  = str(self.serPort.read(1000))
+            #hex read
+            hx = res[res.find("\n"):res.find(">")]
+            # binary string
+            bn =  bin(int(str(hx), 16))[2:][::-1]
+            # read for pins
+            result = {}
+            for pin in pins:
+                if (pin != None):
+                    result[pin]=self.str2bool(bn[pin])
+            return result
+        except Exception as e: 
+                rospy.logwarn("Cannot read serial: {}".format(e))
+                result = {}
+                for pin in pins:
+                    if (pin != None):
+                        result[pin] = False
+                return result
+
+
+    def readMultIOAvg(self,pins=[],readtimes=8):
+        try:
+            result = {}
+            for pin in pins:
+                    if (pin != None):
+                        result[pin]=[]
+            for i in range(readtimes):
+                self.serPort.write("gpio readall\n\r")
+                res = str(self.serPort.read(1000))
+                #hex read
+                hx = res[res.find("\n"):res.find(">")]
+                # binary string
+                bn =  bin(int(str(hx), 16))[2:][::-1]
+                # read for pins
+                for pin in pins:
+                    if (pin != None):
+                        result[pin].append(self.str2bool(bn[pin]))
+
+            result_avg = {}
+            for _pin in result.keys():
+                result_avg[_pin] = self.getAvgBool(result[_pin],_pin)
+            return result_avg
+        except Exception as e: 
+            rospy.logwarn("Cannot read serial: {}".format(e))
+            result = {}
+            for pin in pins:
+                if (pin != None):
+                    result[pin] = False
+            return result
+
     def writeIO(self,pin,state):
-        # tic = rospy.Time.now().to_sec()
         self.serPort.write("gpio "+ CMDS[state] +" "+ self.pin_to_index(pin)  + "\r")
         res  = str(self.serPort.read(self.SERIAL_READ_SIZE))
-        # print("> writeIO at: {}".format(rospy.Time.now().to_sec()-tic))
         return res
 
 
@@ -114,16 +162,12 @@ class Digital_Output(GPIO):
         self.serPort = serPort
         self.msg = Labjack_dout()
         self.msg.header.stamp = rospy.Time.now()
-
-        try:
-            self.rate = configs['outputs'][id]["rate"]
-        except:
-            self.rate = 15.0
+
+        self.rate = 15.0
         self.topic_sub = False
         self.state = False
 
         self.sub = rospy.Subscriber(self.topic_name, Bool, self.callback)
-        self.val_lock = Lock()
 
     def callback(self,data):
         self.topic_sub = True
@@ -149,10 +193,6 @@ class Digital_Constants(GPIO):
         self.msg = Labjack_dout()
         self.msg.header.stamp = rospy.Time.now() 
 
-        try:
-            self.rate = configs['constants'][id]["rate"]
-        except:
-            self.rate = 15.0
         self.sub_flag = True
         self.set = False
         self.update_publish()
@@ -176,7 +216,6 @@ class Digital_Input(GPIO):
 
         self.pin = configs['inputs'][id]["pin"]
         self.logic = configs['inputs'][id]["logic"]
-        self.rate = configs['inputs'][id]["rate"]
 
         self.pub = rospy.Publisher(self.topic_name, Labjack_dout, queue_size=1)
 
@@ -200,7 +239,6 @@ class Digital_Input_bumper_v2(GPIO):
         self.id = id
         self.topic_name = configs['bumper_v2'][id]["topic_name"]
         self.serPort  = serPort
-        self.rate = configs['bumper_v2'][id]['rate']
         self.logic = configs['bumper_v2'][id]["logic"]
         self.msg = Bumper_v2()
         # Low Center Bumper Layer
@@ -256,28 +294,28 @@ class Digital_Input_bumper_v2(GPIO):
     def update_publish(self):
         if (self.pub.get_num_connections()>0):
             # Reading Digital Inputs
-                self.readMultIO()
+                res = self.readMultIOAvg([self.pin_lcb,self.pin_lrb,self.pin_llb,self.pin_hrb,self.pin_hcb,self.pin_hlb,self.pin_sb])
                 msg = Bumper_v2()
                 if (self.pin_lcb!=None):
-                    msg.low_bumper_center = self.get_result(self.readIOAvgBool(self.pin_lcb))
+                    msg.low_bumper_center = self.get_result(res[self.pin_lcb])
 
                 if (self.pin_lrb!=None):
-                    msg.low_bumper_right = self.get_result(self.readIOAvgBool(self.pin_lrb))
+                    msg.low_bumper_right = self.get_result(res[self.pin_lrb])
 
                 if (self.pin_llb!=None):
-                    msg.low_bumper_left = self.get_result(self.readIOAvgBool(self.pin_llb))
+                    msg.low_bumper_left = self.get_result(res[self.pin_llb])
 
                 if (self.pin_hrb!=None):
-                    msg.high_bumper_right = self.get_result(self.readIOAvgBool(self.pin_hrb))
+                    msg.high_bumper_right = self.get_result(res[self.pin_hrb])
 
                 if (self.pin_hcb!=None):
-                    msg.high_bumper_center = self.get_result(self.readIOAvgBool(self.pin_hcb))
+                    msg.high_bumper_center = self.get_result(res[self.pin_hcb])
 
                 if (self.pin_hlb!=None):
-                    msg.high_bumper_left = self.get_result(self.readIOAvgBool(self.pin_hlb))
+                    msg.high_bumper_left = self.get_result(res[self.pin_hlb])
 
                 if (self.pin_sb!=None):
-                    msg.bumper_state = self.get_result(self.readIOAvgBool(self.pin_sb))
+                    msg.bumper_state = self.get_result(res[self.pin_sb])
 
                 # Write values of T4 to message
                 msg.header.stamp = rospy.Time.now()
@@ -307,6 +345,8 @@ class NumatoRelayInterface:
                 sys.exit(0)
 
         self.serial_port = serial.Serial(self.port, self.baud, timeout=self.timeout)
+        self.tmr = None
+
 
     def read_params(self):
         if rospy.has_param("numato") :
@@ -346,19 +386,16 @@ class NumatoRelayInterface:
         return topics
 
 
+    def callback(self, event):
+        for _io in self.objs:
+                _io.update_publish()
+
     def run(self):
         configs = self.read_params()
         self.objs = self.dictionary2objectlist(configs)
-
-        while (not rospy.is_shutdown()):
-            rate = rospy.get_param("~rate",10)
-            tic = rospy.Time.now().to_sec()
-            # if (DEBUG):
-            for _io in self.objs:
-                _io.update_publish()
-            # if (DEBUG):
-            print("== Numato Loop time: {:0.4f} ==".format(rospy.Time.now().to_sec()-tic))
-            rospy.sleep(1.0/float(rate))
+        rate = rospy.get_param("~rate",10)
+        rospy.Timer(rospy.Duration(1.0 / float(rate)), self.callback)
+        rospy.spin()
 
 
 if __name__ == "__main__":