新增beep ring的功能

src/robot_calibration/config/linear_calibrate_params.yaml

@@ -26,7 +26,7 @@
 #  20180911:增加check_rate和cmd_topic两个参数.
 #######################################################################
 test_distance: 2.0  # m
-linear_speed: 0.17  # m/s
+linear_speed: 0.12  # m/s
 tolerance_linear: 0.005  # 0.5cm
 linear_scale: 1.0
 check_rate: 16

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

@@ -22,12 +22,12 @@
 
 import thread
 from math import pi as PI
-import os
-import time
 import sys, traceback
 from serial.serialutil import SerialException
 from serial import Serial
 import rospy
+import time
+import os
 
 
 class Arduino:
@@ -37,7 +37,7 @@ class Arduino:
     N_DIGITAL_PORTS = 54
 
     def __init__(self, port="/dev/ttyACM0", baudrate=57600, timeout=0.5):
-        self.PID_RATE = 30 # Do not change this!  It is a fixed property of the Arduino PID controller.
+        self.PID_RATE = 40 # Do not change this!  It is a fixed property of the Arduino PID controller.
         self.PID_INTERVAL = 1000 / 30
 
         self.port     = port
@@ -65,9 +65,12 @@ 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
+            
+            beep_ring(1)
             print "Connected at", self.baudrate
             print "Arduino is ready."
         except SerialException:
@@ -86,6 +89,7 @@ class Arduino:
     def close(self):
         ''' Close the serial port.
         '''
+        beep_ring(0)
         self.port.close()
 
     def send(self, cmd):
@@ -314,6 +318,9 @@ class Arduino:
     def alarm_write(self, value):
         return self.execute_ack('f %d' %value)
 
+    def beep_ring(self, value):
+        return self.execute_ack('p %d' %value)
+
     def light_show(self, value):
         return self.execute_ack('l %d' %value)