新增读取红外测距传感器的函数在arduino_driver.py代码中

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

@@ -466,6 +466,34 @@ class Arduino:
             except:
                 return None
 
+
+    def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
+        if self.is_use_serial:
+            return self.execute_array('h')
+        else:
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('j')))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+                result_array2 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+                #There are result_flag valid bytes.
+                result_flag2 = result_array2.index(120)
+                #rospy.loginfo("Valid bytes: "+str(result_flag2))
+                #extract result_flag bytes as strings
+                result_string2=''.join([chr(ch) for ch in result_array2[0:(result_flag2-1)]])
+                #extract infrared_counts by space
+                values=[float(s) for s in result_string2.split(" ")]
+                #print values
+                if len(values) != 3:
+                    rospy.logwarn("InfraredEncoder count was not 3")
+                    return None
+                else:
+                    #rospy.loginfo("Infrared counts now: "+ str(values))
+                    return values
+            except:
+                return None
+
+
     def getBatPercent(self):
         '''get the remaining power percentage
         '''

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

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