修正读取编码器和传感器信息时会报错的问题

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -84,11 +84,12 @@ class Arduino:
             test = self.i2c_get_baud()
             #test2 = self.getCurrentValue()
             #test3 = self.getBatPercent()
-            #test4 = self.detect_distance()#wrong
+            #test4 = self.detect_distance()
             #test5 = self.analog_read(1)
             #test6 = self.digital_read(10)
             #test7 = self.i2c_get_pidin()
-            #print test7
+            #test8 = self.i2c_get_encoder_counts()
+            #print test8
             if test != self.baudrate:
                 time.sleep(1)
                 test = self.i2c_get_baud()
@@ -339,7 +340,7 @@ class Arduino:
             result_flag = result_array.index(120)
             #rospy.loginfo("Valid bytes: "+str(result_flag))
             #extract result_flag bytes as strings
-            result_string=''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08, result_flag)])
+            result_string=''.join([chr(ec) for ec in result_array[0:(result_flag-1)]])
             #extract encoder_counts by space
             values=[int(s) for s in result_string.split(" ")]
             if len(values) != 3:
@@ -462,9 +463,9 @@ class Arduino:
                 self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('d'), [ord(c) for c in cmd])
                 pin = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 2)
                 pin = map(chr, pin)
-                print pin
+                #print pin
                 ret = ''.join(pin)
-                print "ret:"+ret
+                #print "ret:"+ret
                 return int(ret)
             except:
                 return None
@@ -584,17 +585,17 @@ class Arduino:
             self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('i')))
             self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
 
-            result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            result_array3 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
             print result_array
             #There are result_flag valid bytes.
-            result_flag = result_array.index(120)
-            print result_flag
+            result_flag3 = result_array.index(120)
+            #print result_flag3
             #rospy.loginfo("Valid bytes: "+str(result_flag))
             #extract result_flag bytes as strings
-            result_string=''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08, result_flag)])
-            print result_string
+            result_string3=''.join([chr(ch) for ch in result_array3[0:(result_flag3-1)]])
+            #print result_string3
             #extract Pidin_counts by space
-            values=[int(s) for s in result_string.split(" ")]
+            values=[int(s) for s in result_string3.split(" ")]
             if len(values) != 3:
                 rospy.logwarn("Pidin was not 3")
                 return None
@@ -626,17 +627,17 @@ class Arduino:
             self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('o')))
             self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
 
-            result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            result_array4 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
             print result_array
             #There are result_flag valid bytes.
-            result_flag = result_array.index(120)
-            print result_flag
+            result_flag4 = result_array.index(120)
+            #print result_flag4
             #rospy.loginfo("Valid bytes: "+str(result_flag))
             #extract result_flag bytes as strings
-            result_string=''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08, result_flag)])
-            print result_string
+            result_string4=''.join([chr(ch) for ch in result_array4[0:(result_flag4-1)]])
+            #print result_string4
             #extract Pidout_counts by space
-            values=[int(s) for s in result_string.split(" ")]
+            values=[int(s) for s in result_string4.split(" ")]
             if len(values) != 3:
                 rospy.logwarn("Pidout was not 3")
                 return None

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -53,8 +53,13 @@ class Sensor(object):
                     self.value = self.read_value()
                     #rospy.loginfo("read value: "+str(self.value))
                 except:
-                    rospy.logerr("Sensor read value error !")
-                    return
+                    try:
+                        # try again
+                        rospy.logwarn("Failed to read sensor values, try again. ")
+                        self.ack = self.write_value()
+                    except:
+                        rospy.logerr("Sensor read value error !")
+                        return
             else:
                 try:
                     self.ack = self.write_value()

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -175,9 +175,14 @@ class BaseController:
                     aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
                     #rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
             except:
-                self.bad_encoder_count += 1
-                rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
-                return
+                try:
+                    # try again
+                    #rospy.logwarn("Failed to get encoder counts, try again. ")
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
+                except:
+                    self.bad_encoder_count += 1
+                    rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
+                    return
 
             # Calculate odometry
             dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter