提交超声波串口部分，发布于话题/US025

src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -10,6 +10,7 @@ add_message_files(FILES
                   Digital.msg
                   SensorState.msg
                   InfraredSensors.msg
+                  UltrasonicSensors.msg
                  )
 
 add_service_files(FILES
@@ -25,6 +26,7 @@ add_service_files(FILES
                   GripperControl.srv
                   GripperAngle.srv
                   GetInfraredDistance.srv
+                  GetUltrasonicDistance.srv
                   DynamicPID.srv
                  )
 

src/ros_arduino_bridge/ros_arduino_msgs/msg/UltrasonicSensors.msg

@@ -0,0 +1,13 @@
+# Float message from three ultrasonic sensors IO pin.
+std_msgs/Header header
+
+uint8 ULTRASONIC=3
+uint8 radiation_type
+
+float32 field_of_view
+float32 min_range
+float32 max_range
+
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_msgs/srv/GetUltrasonicDistance.srv

@@ -0,0 +1,4 @@
+---
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -13,6 +13,7 @@ is_use_serial: True
 
 # /dev/ttyACM# where is # is a number such as 0, 1 etc
 serial_port: /dev/ttyACM0
+#serial_port: /dev/ttyS0
 serial_baud: 57600
 
 i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
@@ -62,6 +63,7 @@ CWheel_Ko: 50
 # Sensor definitions (case sensitive!):
 sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
+  US025: {pin: 0, type: US025, rate: 5, direction: input},
   batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1, direction: input},
 }
 

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -80,6 +80,9 @@ class ArduinoROS():
         # A service to return infrared sensor distance
         rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
 
+        # A service to return ultrasonic sensor distance
+        rospy.Service('~getUltrasonicDistance', GetUltrasonicDistance, self.GetUltrasonicDistanceHandler)
+
         # Initialize the controlller
         self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
@@ -112,8 +115,10 @@ class ArduinoROS():
                 sensor = BatPercent(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'MotorTotalCurrent':
                 sensor = MotorTotalCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
-            elif params['type'] == 'CurrentValue':
-                sensor = CurrentValue(self.controller, name, params['pin'], params['rate'], self.base_frame)
+#            elif params['type'] == 'CurrentValue':
+#                sensor = CurrentValue(self.controller, name, params['pin'], params['rate'], self.base_frame)
+            elif params['type'] == "US025":
+                sensor = US025(self.controller, name, params['pin'], params['rate'], self.base_frame)
 
             try:
                 self.mySensors.append(sensor)
@@ -177,6 +182,10 @@ class ArduinoROS():
         value = self.controller.detect_distance()
         return GetInfraredDistanceResponse(value[0]/100.0,value[1]/100.0,value[2]/100.0)
 
+    def GetUltrasonicDistanceHandler(self, req):
+        value = self.controller.detect_distance_ultrasonic()
+        return GetUltrasonicDistanceResponse(value[0]/100.0,value[1]/100.0,value[2]/100.0)
+
     def DynamicPIDHandler(self, req):
         self.controller.update_pid(req.value1, req.value2, 0, 50,
                                    req.value3, req.value4, 0, 50,

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

@@ -526,6 +526,12 @@ class Arduino:
             except:
                 return None
 
+    def detect_distance_ultrasonic(self): #检测三个超声波测距传感器的返回值,一共返回三个值
+        if self.is_use_serial:
+            return self.execute_array('f')
+        else:
+            return None
+
 
     def getBatPercent(self):
         '''get the remaining power percentage
@@ -546,24 +552,22 @@ class Arduino:
             except:
                 return None
 
-    def getCurrentValue(self):
-        '''get current value
-        '''
-        if self.is_use_serial:
-            currentvalue = self.execute('f')
-            #rospy.loginfo("Current value:" + str(currentvalue))
-            return currentvalue
-        else:
-            try:
-                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('f'))
-                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
-                currentvalue = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
-                currentvalue = map(chr, currentvalue)
-                ret = ''.join(currentvalue)
-                #print "ret:"+ret
-                return float(ret)
-            except:
-                return None
+#    def getCurrentValue(self):
+#        if self.is_use_serial:
+#            currentvalue = self.execute('f')
+#            #rospy.loginfo("Current value:" + str(currentvalue))
+#            return currentvalue
+#        else:
+#            try:
+#                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('f'))
+#                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+#                currentvalue = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
+#                currentvalue = map(chr, currentvalue)
+#                ret = ''.join(currentvalue)
+#                #print "ret:"+ret
+#                return float(ret)
+#            except:
+#                return None
 
     def get_pidin(self):
         values = self.execute_array('i')

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

@@ -22,13 +22,14 @@ INPUT = 0
 OUTPUT = 1
 
 class MessageType:
-    ANALOG   = 0
-    DIGITAL  = 1
-    INFRARED = 2
-    RANGE    = 3
-    FLOAT    = 4
-    INT      = 5
-    BOOL     = 6
+    ANALOG     = 0
+    DIGITAL    = 1
+    INFRARED   = 2
+    RANGE      = 3
+    FLOAT      = 4
+    INT        = 5
+    BOOL       = 6
+    ULTRASONIC = 7
 
 class Sensor(object):
     def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
@@ -74,6 +75,10 @@ class Sensor(object):
                 self.msg.front_dist = self.value[0]
                 self.msg.left_dist  = self.value[1]
                 self.msg.right_dist = self.value[2]
+            elif self.message_type == MessageType.ULTRASONIC:
+                self.msg.front_dist = self.value[0]
+                self.msg.left_dist  = self.value[1]
+                self.msg.right_dist = self.value[2]
             else:
                 self.msg.value = self.value
 
@@ -173,6 +178,19 @@ class SonarSensor(RangeSensor):
         super(SonarSensor, self).__init__(*args, **kwargs)
         self.msg.radiation_type = Range.ULTRASOUND
 
+class MyRangeSensor2(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(MyRangeSensor2, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.ULTRASONIC
+        self.msg = UltrasonicSensors()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, UltrasonicSensors, queue_size=2)
+
+    def read_value(self):
+        self.msg.header.stamp = rospy.Time.now()
+
 class IRSensor(RangeSensor):
     def __init__(self, *args, **kwargs):
         super(IRSensor, self).__init__(*args, **kwargs)
@@ -223,6 +241,19 @@ class IR2Y0A02(IRSensor):
 
         return dist
 
+class US025(MyRangeSensor2):
+    def __init__(self, *args, **kwargs):
+        super(US025, self).__init__(*args, **kwargs)
+
+        self.msg.field_of_view = 0.01
+        self.msg.min_range = 0.03
+        self.msg.max_range = 4
+
+    def read_value(self):
+        values = self.controller.detect_distance_ultrasonic()
+        distances = np.array([values[0], values[1], values[2]])
+        return distances/100.0
+
 class MotorTotalCurrent(AnalogFloatSensor):
     def __init__(self, *args, **kwargs):
         super(MotorTotalCurrent, self).__init__(*args, **kwargs)
@@ -240,6 +271,7 @@ class BatPercent(DigitalSensor):
         percent = self.controller.getBatPercent()
         return int(percent)
 
+'''
 class CurrentValue(DigitalSensor):
     def __init__(self, *args, **kwargs):
         super(CurrentValue, self).__init__(*args, **kwargs)
@@ -248,7 +280,7 @@ class CurrentValue(DigitalSensor):
         currentvalue = self.controller.getCurrentValue()
         #rospy.loginfo("read currentvalue: "+currentvalue)
         return float(currentvalue)
-
+'''
 
 if __name__ == '__main__':
     rospy.loginf("arduino sensor main function ...")