调试好红外测距传感器的话题输出

.gitignore

@@ -1,2 +1,3 @@
 build/
 devel/
+*.pyc

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -9,6 +9,7 @@ add_message_files(FILES
                   ArduinoConstants.msg
                   Digital.msg
                   SensorState.msg
+                  InfraredSensors.msg
                  )
 
 add_service_files(FILES

src/ros_arduino_bridge/ros_arduino_msgs/msg/InfraredSensors.msg

@@ -1,6 +1,13 @@
 # Float message from three infrared sensors IO pin.
-Header header
+std_msgs/Header header
 
-float64 front_dist
-float64 left_dist
-float64 right_dist
+uint8 INFRARED=1
+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_python/config/my_arduino_params.yaml

@@ -12,7 +12,6 @@ i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 timeout: 0.5
 
 rate: 25
-sensorstate_rate: 10
 
 cmd_topic: cmd_vel
 
@@ -50,12 +49,10 @@ CWheel_Kd: 15
 CWheel_Ki: 0
 CWheel_Ko: 50
 
-# === Sensor definitions.  Examples only - edit for your robot.
-#     Sensor type can be one of the follow (case sensitive!):
-#	  * GP2D12
+# === Sensor definitions.Sensor type can be one of the follow (case sensitive!):
 #	  * Analog
 #	  * Digital
 sensors: {
-  #antiDropIR_front: {pin: 22, type: Digital, rate: 5, direction: input},
+  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 10, direction: input},
 }
 

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -7,12 +7,12 @@
 import os
 import rospy
 import thread
-from ros_arduino_python.arduino_driver import Arduino
-from ros_arduino_python.arduino_sensors import *
 from ros_arduino_msgs.srv import *
-from ros_arduino_python.base_controller import BaseController
 from geometry_msgs.msg import Twist
 from serial.serialutil import SerialException
+from ros_arduino_python.arduino_sensors import *
+from ros_arduino_python.arduino_driver import Arduino
+from ros_arduino_python.base_controller import BaseController
 
 class ArduinoROS():
     def __init__(self):
@@ -37,24 +37,12 @@ class ArduinoROS():
         self.rate = int(rospy.get_param("~rate", 20))
         loop_rate = rospy.Rate(self.rate)
 
-        # Rate at which summary SensorState message is published. Individual sensors publish
-        # at their own rates.
-        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
-
-        # Set up the time for publishing the next SensorState message
-        now = rospy.Time.now()
-        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
-        self.t_next_sensors  = now + self.t_delta_sensors
-
         # Initialize a Twist message
         self.cmd_vel = Twist()
 
         # A cmd_vel publisher so we can stop the robot when shutting down
         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=20)
 
-        # The publisher periodically publishes the values of all sensors on a single topic.
-        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
-
         # A service to turn set the direction of a digital pin (0 = input, 1 = output)
         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
 
@@ -81,7 +69,6 @@ class ArduinoROS():
 
         # Make the connection
         self.controller.connect()
-        #rospy.loginfo("Connected to Arduino on port " + self.serial_port + " at " + str(self.serial_baud) + " serial baud !")
 
         # Reserve a thread lock
         mutex = thread.allocate_lock()
@@ -97,12 +84,10 @@ class ArduinoROS():
             except:
                 params['direction'] = 'input'
 
-            if params['type'] == "GP2D12":
-                sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
+            if params['type'] == "GP2Y0A41":
+                sensor = GP2Y0A41(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == "IR2Y0A02":
                 sensor = IR2Y0A02(self.controller, name, params['pin'], params['rate'], self.base_frame)
-            elif params['type'] == "GP2Y0A41":
-                sensor = GP2Y0A41(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'Digital':
                 sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
             elif params['type'] == 'Analog':
@@ -123,7 +108,6 @@ class ArduinoROS():
 
         # Start polling the sensors and base controller
         while not rospy.is_shutdown():
-            # Base controller
             mutex.acquire()
             self.myBaseController.poll()
             mutex.release()
@@ -133,22 +117,6 @@ class ArduinoROS():
                 sensor.poll()
                 mutex.release()
 
-            # Publish all sensor values on a single topic for convenience
-            now = rospy.Time.now()
-            if now > self.t_next_sensors:
-                msg = SensorState()
-                msg.header.frame_id = self.base_frame
-                msg.header.stamp    = now
-                for i in range(len(self.mySensors)):
-                    msg.name.append(self.mySensors[i].name)
-                    msg.value.append(self.mySensors[i].value)
-                try:
-                    self.sensorStatePub.publish(msg)
-                except:
-                    pass
-
-                self.t_next_sensors = now + self.t_delta_sensors
-
             loop_rate.sleep()
 
     def DigitalSetDirectionHandler(self, req):

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

@@ -1,14 +1,15 @@
 #!/usr/bin/env python
+#-*- coding:utf-8 -*-
 
 """
-    A Python driver for the Arduino microcontroller running the
-    ROSArduinoBridge firmware.
+    A Python driver for the Arduino microcontroller.
 """
 
 from math import pi as PI
 import sys, traceback
 from serial.serialutil import SerialException
 from serial import Serial
+import numpy as np
 import thread
 import smbus
 import rospy
@@ -22,15 +23,16 @@ class Arduino:
     N_ANALOG_PORTS  = 10
     N_DIGITAL_PORTS = 54
 
-    def __init__(self, is_use_serial, serial_port="/dev/ttyACM0", baudrate=57600,i2c_smbus_num=1, i2c_slave_addr=8, timeout=0.5):
-        self.PID_RATE = 40 # Do not change this!  It is a fixed property of the Arduino PID controller.
+    def __init__(self, is_use_serial,serial_port="/dev/ttyACM0",baudrate=57600,i2c_smbus_num=1,i2c_slave_addr=8,timeout=0.5):
+        self.PID_RATE = 40   #Don't change this ! It is a fixed property of the Arduino PID controller.
         self.PID_INTERVAL = 25
 
-        self.is_use_serial = is_use_serial
-        self.serial_port   = serial_port
-        self.baudrate = baudrate
+        self.is_use_serial = is_use_serial #与下位机arduino的通信方式是否使用串口还是IIC
 
-        self.i2c_smbus_num = i2c_smbus_num
+        self.serial_port = serial_port
+        self.baudrate    = baudrate
+
+        self.i2c_smbus_num  = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
 
         self.timeout  = timeout
@@ -64,7 +66,6 @@ class Arduino:
             test = self.get_baud()
             if test != self.baudrate:
                 time.sleep(1)
-                print "Connecting ..."
                 test = self.get_baud()
                 if test != self.baudrate:
                     raise SerialException
@@ -82,7 +83,6 @@ class Arduino:
 
     def i2c_connect(self):
         try:
-
             print "Connecting to Arduino on IIC addr ", self.i2c_slave_addr, "..."
             test = self.i2c_get_baud()
             if test != self.baudrate:
@@ -102,18 +102,20 @@ class Arduino:
             print "Cannot connect to Arduino IIC slave addr !"
             os._exit(1)
 
-
     def open(self):
         ''' Open the serial port.
         '''
         self.serial_port.open()
 
     def close(self):
-        ''' Close the serial port.
+        ''' Close the serial port or i2c bus connect.
         '''
+        print "Now disconnecting ..."
         self.beep_ring(0)
-        self.serial_port.close()
-        self.i2c_bus.close()
+        if self.is_use_serial:
+            self.serial_port.close()
+        else:
+            self.i2c_bus.close()
 
     def send(self, cmd):
         ''' This command should not be used on its own: it is called by the execute commands
@@ -163,7 +165,7 @@ class Arduino:
         '''
         try:
             values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
-            return map(int, values)
+            return map(float, values)
         except:
             return []
 
@@ -179,7 +181,6 @@ class Arduino:
 
         ntries   = 1
         attempts = 0
-
         try:
             self.serial_port.write(cmd + '\r')
             value = self.recv(self.timeout)
@@ -200,7 +201,7 @@ class Arduino:
         return value
 
     def execute_array(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning an array.
+        ''' Thread safe execution of "cmd" on the Arduino returning an float array.
         '''
         self.mutex.acquire()
         try:
@@ -228,7 +229,7 @@ class Arduino:
             return []
 
         try:
-            values = map(int, values)
+            values = map(float, values)
         except:
             values = []
 
@@ -247,7 +248,6 @@ class Arduino:
 
         ntries   = 1
         attempts = 0
-
         try:
             self.serial_port.write(cmd + '\r')
             ack = self.recv(self.timeout)
@@ -271,7 +271,7 @@ class Arduino:
     def update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
                          BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,
                          CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
-        ''' Set the PID parameters on the Arduino
+        ''' Set the PID parameters on the Arduino by serial
         '''
         print "Updating PID parameters"
         cmd = 'u '+str(AWheel_Kp)+':'+str(AWheel_Kd)+':'+str(AWheel_Ki)+':'+str(AWheel_Ko)+':'+str(BWheel_Kp)+':'+str(BWheel_Kd)+':'+str(BWheel_Ki)+':'+str(BWheel_Ko)+':'+str(CWheel_Kp)+':'+str(CWheel_Kd)+':'+str(CWheel_Ki)+':'+str(CWheel_Ko)
@@ -339,7 +339,6 @@ class Arduino:
         else:
             return values
 
-
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port
         '''
@@ -370,19 +369,6 @@ class Arduino:
         except:
             return None
 
-    def drive_m_per_s(self, AWheel, BWheel, CWheel):
-        ''' Set the motor speeds in meters per second.
-        '''
-        aWheel_revs_per_second = float(AWheel) / (self.wheel_diameter * PI)
-        bWheel_revs_per_second = float(BWheel) / (self.wheel_diameter * PI)
-        cWheel_revs_per_second = float(CWheel) / (self.wheel_diameter * PI)
-
-        aWheel_ticks_per_loop = int(aWheel_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
-        bWheel_ticks_per_loop = int(bWheel_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
-        cWheel_ticks_per_loop = int(cWheel_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
-
-        self.drive(aWheel_ticks_per_loop , bWheel_ticks_per_loop, cWheel_ticks_per_loop)
-
     def stop(self):
         ''' Stop all three motors.
         '''
@@ -410,9 +396,6 @@ class Arduino:
     def pin_mode(self, pin, mode):
         return self.execute_ack('c %d %d' %(pin, mode))
 
-    def alarm_write(self, value):
-        return self.execute_ack('f %d' %value)
-
     def beep_ring(self, value):
         return self.execute_ack('p %d' %value)
 
@@ -422,6 +405,9 @@ class Arduino:
     def detect_current(self):
         return self.execute('f')
 
+    def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
+        return self.execute_array('h')
+
     def light_show(self, value):
         return self.execute_ack('l %d' %value)
 
@@ -463,28 +449,33 @@ class Arduino:
         #else:
         #    return values
 
-""" Basic test for connectivity """
+""" Basic test for connectivity by serial port or IIC bus"""
 if __name__ == "__main__":
+    is_use_serial = True
     portName = "/dev/ttyACM0"
     baudRate = 57600
 
-    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
+    iic_smbus = 1
+    iic_num = 8
+
+    myArduino = Arduino(is_use_serial, serial_port=portName, baudrate=baudRate, i2c_smbus_num=iic_smbus, i2c_slave_addr=iic_num, timeout=0.5)
     myArduino.connect()
 
     print "Sleeping for 1 second..."
     time.sleep(1)
 
-    print "Reading on analog port 0", myArduino.analog_read(0)
-    print "Reading on digital port 0", myArduino.digital_read(0)
-    print "Blinking the LED 3 times"
+    print "Test Beep ring 3 times..."
     for i in range(3):
-        myArduino.digital_write(13, 1)
-        time.sleep(1.0)
-    #print "Current encoder counts", myArduino.encoders()
-    print "Connection test successful.",
+        myArduino.beep_ring(1)
+        time.sleep(2.0)
+
+    print "Did you heard the beep ring ?"
+    print "Now print infrared sensors value:"
+    values = myArduino.detect_distance()
+    distances = np.array([values[0], values[1], values[2]])
+    print distances/100.0
 
     myArduino.stop()
     myArduino.close()
-
     print "Shutting down Arduino."
 

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

@@ -6,10 +6,12 @@
 
 import roslib; roslib.load_manifest('ros_arduino_python')
 import rospy
+import numpy as np
 from decimal import Decimal
 from sensor_msgs.msg import Range
 from ros_arduino_msgs.msg import *
 
+
 LOW = 0
 HIGH = 1
 
@@ -17,12 +19,13 @@ INPUT = 0
 OUTPUT = 1
 
 class MessageType:
-    ANALOG  = 0
-    DIGITAL = 1
-    RANGE   = 2
-    FLOAT   = 3
-    INT     = 4
-    BOOL    = 5
+    ANALOG   = 0
+    DIGITAL  = 1
+    INFRARED = 2
+    RANGE    = 3
+    FLOAT    = 4
+    INT      = 5
+    BOOL     = 6
 
 class Sensor(object):
     def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
@@ -55,6 +58,10 @@ class Sensor(object):
             # For range sensors, assign the value to the range message field
             if self.message_type == MessageType.RANGE:
                 self.msg.range = self.value
+            elif self.message_type == MessageType.INFRARED:
+                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
 
@@ -112,7 +119,6 @@ class AnalogFloatSensor(Sensor):
     def write_value(self, value):
         return self.controller.analog_write(self.pin, value)
 
-
 class DigitalSensor(Sensor):
     def __init__(self, *args, **kwargs):
         super(DigitalSensor, self).__init__(*args, **kwargs)
@@ -144,27 +150,54 @@ class RangeSensor(Sensor):
         super(RangeSensor, self).__init__(*args, **kwargs)
 
         self.message_type = MessageType.RANGE
-
         self.msg = Range()
         self.msg.header.frame_id = self.frame_id
-
         self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
 
     def read_value(self):
         self.msg.header.stamp = rospy.Time.now()
 
+class MyRangeSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(MyRangeSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.INFRARED
+        self.msg = InfraredSensors()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, InfraredSensors, queue_size=2)
+
+    def read_value(self):
+        self.msg.header.stamp = rospy.Time.now()
+
 class SonarSensor(RangeSensor):
     def __init__(self, *args, **kwargs):
         super(SonarSensor, self).__init__(*args, **kwargs)
-
         self.msg.radiation_type = Range.ULTRASOUND
 
 class IRSensor(RangeSensor):
     def __init__(self, *args, **kwargs):
         super(IRSensor, self).__init__(*args, **kwargs)
+        self.msg.radiation_type = Range.INFRARED
 
+class MyIRSensor(MyRangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(MyIRSensor, self).__init__(*args, **kwargs)
         self.msg.radiation_type = Range.INFRARED
 
+class GP2Y0A41(MyIRSensor):
+    def __init__(self, *args, **kwargs):
+        super(GP2Y0A41, self).__init__(*args, **kwargs)
+
+        self.msg.field_of_view = 0.01
+        self.msg.min_range = 0.04
+        self.msg.max_range = 0.30
+
+    def read_value(self):
+        values = self.controller.detect_distance()
+        distances = np.array([values[0], values[1], values[2]])
+        return distances/100.0
+
 class IR2Y0A02(IRSensor):
     def __init__(self, *args, **kwargs):
         super(IR2Y0A02, self).__init__(*args, **kwargs)
@@ -192,53 +225,6 @@ class IR2Y0A02(IRSensor):
 
         return dist
 
-class GP2Y0A41(IRSensor):
-    def __init__(self, *args, **kwargs):
-        super(GP2Y0A41, self).__init__(*args, **kwargs)
-
-        self.msg.field_of_view = 0.001
-        self.msg.min_range = 0.04
-        self.msg.max_range = 0.30
-
-    def read_value(self):
-        value = self.controller.analog_read(self.pin)
-
-        dist = 20.76/(value - 11.0)
-
-        # If we get a spurious reading, set it to the max_range
-        if dist > self.msg.max_range: dist = self.msg.max_range
-        if dist < self.msg.min_range: dist = self.msg.min_range
-
-        return Decimal(dist).quantize(Decimal('0.000'))
-
-class GP2D12(IRSensor):
-    def __init__(self, *args, **kwargs):
-        super(GP2D12, self).__init__(*args, **kwargs)
-
-        self.msg.field_of_view = 0.001
-        self.msg.min_range = 0.10
-        self.msg.max_range = 0.80
-
-    def read_value(self):
-        value = self.controller.analog_read(self.pin)
-
-        if value <= 3.0:
-            return self.msg.max_range
-
-        try:
-            distance = (6787.0 / (float(value) - 3.0)) - 4.0
-        except:
-            return self.msg.max_range
-
-        # Convert to meters
-        distance /= 100.0
-
-        # If we get a spurious reading, set it to the max_range
-        if distance > self.msg.max_range: distance = self.msg.max_range
-        if distance < self.msg.min_range: distance = self.msg.max_range
-
-        return distance
-
 class MotorTotalCurrent(AnalogFloatSensor):
     def __init__(self, *args, **kwargs):
         super(MotorTotalCurrent, self).__init__(*args, **kwargs)