From e83657174fd33799376be927748f101fc9c840c1 Mon Sep 17 00:00:00 2001 From: Michalis Logothetis Date: Fri, 15 Dec 2023 13:46:57 +0200 Subject: [PATCH 1/2] remove prints and add multiple IOs check with averaging --- scripts/numato_ros | 153 ++++++++++++++++++++++++++++----------------- 1 file changed, 95 insertions(+), 58 deletions(-) diff --git a/scripts/numato_ros b/scripts/numato_ros index 83fc87d..fe9397d 100755 --- a/scripts/numato_ros +++ b/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__": From 54f9fe4a13a9ca927685f1ed47a467b52156dba8 Mon Sep 17 00:00:00 2001 From: Michalis Logothetis Date: Fri, 15 Dec 2023 13:47:46 +0200 Subject: [PATCH 2/2] remove rates from yaml --- config/io.yaml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/config/io.yaml b/config/io.yaml index a5894c9..9345c11 100644 --- a/config/io.yaml +++ b/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 \ No newline at end of file