为添加两轮平衡车模式更新ros代码

.gitignore

@@ -2,3 +2,4 @@ build/
 devel/
 *.pyc
 *.so
+.vscode/

.vscode/c_cpp_properties.json

@@ -1,39 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "",
-        "limitSymbolsToIncludedHeaders": true
-      },
-      "includePath": [
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/devel/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_vision_ws/devel/include/**",
-        "/opt/ros/melodic/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/amcl/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/base_local_planner/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/carrot_planner/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/clear_costmap_recovery/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/costmap_2d/include/**",
-        "/home/ssfz/cv_bridge_ws/src/vision_opencv/cv_bridge/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/dwa_local_planner/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/global_planner/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_camera/gscam/include/**",
-        "/home/ssfz/cv_bridge_ws/src/vision_opencv/image_geometry/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/map_server/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/move_base/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/move_slow_and_clear/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/nav_core/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/navfn/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/qingzhou_odom/qingzhou_bringup/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/qingzhou_odom/robot_pose_ekf/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_race/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/rotate_recovery/include/**",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/src/robot_navagition/voxel_grid/include/**",
-        "/usr/include/**",
-        "/home/ssfz/work/ssfz/panda_ws/src/**"
-      ],
-      "name": "ROS"
-    }
-  ],
-  "version": 4
-}

.vscode/settings.json

@@ -1,83 +0,0 @@
-{
-    "python.autoComplete.extraPaths": [
-        "/home/ssfz/cv_bridge_ws/devel/lib/python3/dist-packages",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/devel/lib/python2.7/dist-packages",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_vision_ws/devel/lib/python2.7/dist-packages",
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ],
-    "python.analysis.extraPaths": [
-        "/home/ssfz/cv_bridge_ws/devel/lib/python3/dist-packages",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_ws/devel/lib/python2.7/dist-packages",
-        "/home/ssfz/work/ssfz/qingzhou/qingzhou_vision_ws/devel/lib/python2.7/dist-packages",
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ],
-    "files.associations": {
-        "*.h": "cpp",
-        "cctype": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "csignal": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "any": "cpp",
-        "array": "cpp",
-        "atomic": "cpp",
-        "strstream": "cpp",
-        "*.tcc": "cpp",
-        "bitset": "cpp",
-        "chrono": "cpp",
-        "cinttypes": "cpp",
-        "complex": "cpp",
-        "condition_variable": "cpp",
-        "cstdint": "cpp",
-        "deque": "cpp",
-        "forward_list": "cpp",
-        "list": "cpp",
-        "unordered_map": "cpp",
-        "unordered_set": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "functional": "cpp",
-        "iterator": "cpp",
-        "map": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "set": "cpp",
-        "string": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "hash_map": "cpp",
-        "hash_set": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "ostream": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "cfenv": "cpp",
-        "typeindex": "cpp",
-        "typeinfo": "cpp"
-    }
-}

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -1,4 +1,4 @@
-# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:arduino节点运行时加载的参数,下面对参数进行解释:
 #   is_use_serial:与arduino通信是否使用串口,false的话就是IIC通信
 #   serial_port:串口通信时arduino的端口号
@@ -18,22 +18,17 @@ serial_baud: 57600
 i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
 i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 
-timeout: 0.5
-
-rate: 25
+connect_timeout: 0.5
 
+main_rate: 50
 cmd_topic: cmd_vel
 
-base_controller_rate: 15.0
-base_controller_timeout: 0.7
-
-# For a robot that uses base_footprint, change base_frame to base_footprint
 base_frame: base_footprint
 odom_name: odom
 
 # Robot drivetrain parameters
 wheel_diameter: 0.067
-wheel_track: 0.21     #L value
+wheel_track: 0.21      #L value
 encoder_resolution: 11 #12V DC motors
 gear_reduction: 30     #empty payload rpm 330 r/m,if rpm=530,gear_reduction=19
 linear_scale_correction: 1.00
@@ -42,7 +37,7 @@ angular_scale_correction: 1.00
 # === PID parameters
 debugPID: False
 
-accel_limit: 0.05
+accel_limit: 0.1
 
 AWheel_Kp: 18
 AWheel_Kd: 18
@@ -59,4 +54,3 @@ sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, 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

@@ -21,30 +21,25 @@ from ros_arduino_python.base_controller import BaseController
 class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
-
-        # Get the actual node name in case it is set in the launch file
         self.name = rospy.get_name()
 
-        # Cleanup when termniating the node
+        #当节点退出的时候执行的回调函数
         rospy.on_shutdown(self.shutdown)
 
-        self.is_use_serial = rospy.get_param("~is_use_serial", True)
-
-        self.serial_port = rospy.get_param("~serial_port", "/dev/ttyACM0")
-        self.serial_baud = int(rospy.get_param("~serial_baud", 57600))
-        self.i2c_smbus_num = rospy.get_param("~i2c_smbus_num", 1)
+        self.is_use_serial  = rospy.get_param("~is_use_serial", True)
+        self.serial_port    = rospy.get_param("~serial_port", "/dev/ttyACM0")
+        self.serial_baud    = int(rospy.get_param("~serial_baud", 57600))
+        self.i2c_smbus_num  = rospy.get_param("~i2c_smbus_num", 1)
         self.i2c_slave_addr = rospy.get_param("~i2c_slave_addr", 8)
-        self.timeout    = rospy.get_param("~timeout", 0.7)
+        self.timeout    = rospy.get_param("~connect_timeout", 0.5)
         self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
 
-        # Overall loop rate: should be faster than fastest sensor rate
-        self.rate = int(rospy.get_param("~rate", 25))
+        #根据参数中配置的频率来设置主循环频率
+        self.rate = int(rospy.get_param("~main_rate", 55))
         loop_rate = rospy.Rate(self.rate)
 
         # 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=1)
 
         # A service to turn set the direction of a digital pin (0 = input, 1 = output)
@@ -74,13 +69,13 @@ class ArduinoROS():
         # 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)
 
-        # Make the connection
+        #与下位机建立连接
         self.controller.connect()
 
         # Reserve a thread lock
         mutex = thread.allocate_lock()
 
-        # Initialize any sensors
+        #获取传感器列表
         self.mySensors = list()
         sensor_params = rospy.get_param("~sensors", dict({}))
 
@@ -93,16 +88,12 @@ class ArduinoROS():
 
             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'] == 'Digital':
                 sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
             elif params['type'] == 'Analog':
                 sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
             elif params['type'] == 'BAT_PERCENT':
                 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'] == "US025":
                 sensor = US025(self.controller, name, params['pin'], params['rate'], self.base_frame)
 
@@ -113,9 +104,9 @@ class ArduinoROS():
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
 
         # Initialize the base controller
-        self.myBaseController = BaseController(self.is_use_serial, self.controller, self.base_frame, self.name + "_base_controller")
+        self.myBaseController = BaseController(self.is_use_serial, self.controller, self.base_frame)
 
-        # Start polling the sensors and base controller
+        #开始主循环,控制底盘和发布传感器信息
         while not rospy.is_shutdown():
             mutex.acquire()
             self.myBaseController.poll()
@@ -148,10 +139,6 @@ class ArduinoROS():
         value = self.controller.analog_read(req.pin)
         return AnalogReadResponse(value)
 
-    def AlarmWriteHandler(self, req):
-        self.controller.alarm_write(req.value)
-        return AlarmWriteResponse()
-
     def LightShowHandler(self, req):
         self.controller.light_show(req.value)
         return LightShowResponse()
@@ -169,7 +156,7 @@ class ArduinoROS():
                                    req.value3, req.value4, 0, 50)
         return DynamicPIDResponse()
 
-    # Stop the robot arduino connect
+    #断开与下位机的连接
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")
         try:
@@ -179,12 +166,8 @@ class ArduinoROS():
             pass
 
         # Close the serial port or IIC
-        try:
-            self.controller.close()
-        except:
-            pass
-        finally:
-            os._exit(0)
+        self.controller.close()
+        os._exit(0)
 
 if __name__ == '__main__':
     try:
@@ -195,4 +178,3 @@ if __name__ == '__main__':
     else:
         rospy.logerr("Get other unknown error !")
         os._exit(0)
-

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

@@ -7,13 +7,13 @@
   Author: corvin, jally
   History:
     20200706:创建代码.
+    20210913:为了两轮平衡车模式优化代码.
 """
 from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
 from serial import Serial
 import sys, traceback
-import numpy as np
-import roslib, rospy
+import rospy
 
 
 class Arduino:
@@ -34,10 +34,8 @@ class Arduino:
         self.i2c_slave_addr = i2c_slave_addr
 
         self.timeout = timeout
-
-        self.encoder_count    = 0
         self.writeTimeout     = timeout
-        self.interCharTimeout = timeout/30.
+        self.interCharTimeout = timeout/30.0
 
         # Keep things thread safe
         self.mutex = thread.allocate_lock()
@@ -50,7 +48,7 @@ class Arduino:
             self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
             self.i2c_connect()
 
-    #使用串口连接进行通信
+    #使用串口建立连接进行通信
     def serial_connect(self):
         try:
             rospy.loginfo("Connecting to Arduino on port: " + self.serial_port)
@@ -286,8 +284,6 @@ class Arduino:
         rospy.loginfo("Updating PID parameters by IIC")
 
     def get_baud(self):
-        ''' Get the current baud rate on the serial port.
-        '''
         try:
             return int(self.execute('b'));
         except:
@@ -347,13 +343,9 @@ class Arduino:
             return None
 
     def reset_encoders(self):
-        ''' Reset the encoder counts to 0 by serial port
-        '''
         return self.execute_ack('r')
 
     def i2c_reset_encoders(self):
-        ''' Reset the encoder counts to 0 by IIC
-        '''
         try:
             self.i2c_bus.write_byte(self.i2c_slave_addr, ord('r'))
             self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
@@ -377,8 +369,6 @@ class Arduino:
                 return None
 
     def drive(self, AWheel, BWheel):
-        ''' Speeds are given in encoder ticks per PID interval
-        '''
         #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel))
         return self.execute_ack('m %d %d' %(AWheel, BWheel))
 
@@ -394,8 +384,6 @@ class Arduino:
 
     #使用串口发送停止电机转动命令
     def stop(self):
-        ''' Stop all three motors.
-        '''
         self.drive(0, 0)
 
     def i2c_stop(self):
@@ -618,21 +606,12 @@ if __name__ == "__main__":
     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()
 
-    rospy.loginfo("Sleeping for 1 second...")
-    time.sleep(1)
-
-    rospy.loginfo("Test Beep ring 3 times...")
+    print("Test Beep ring 3 times...")
     for i in range(3):
         myArduino.beep_ring(1)
         time.sleep(2.0)
-
-    rospy.loginfo("Did you heard the beep ring ?")
-    rospy.loginfo("Now print infrared sensors value:")
-    values = myArduino.detect_distance()
-    distances = np.array([values[0], values[1], values[2]])
-    rospy.loginf(distances/100.0)
+    print("Did you heard the beep ring ?")
 
     myArduino.stop()
     myArduino.close()
-    rospy.logwarn("Shutting down Arduino !")
-
+    print("Shutting down Arduino !")

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

@@ -219,33 +219,6 @@ class GP2Y0A41(MyIRSensor):
         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)
-
-        self.msg.field_of_view = 0.001
-        self.msg.min_range = 0.200
-        self.msg.max_range = 1.500
-
-    def read_value(self):
-        value = self.controller.analog_read(self.pin)
-
-        try:
-            volts = value*0.0048828125;
-            distance = 65*pow(volts, -1.10)
-        except:
-            return self.msg.min_range
-
-        # Convert to meters
-        distance /= 100.000
-        dist = round(float(distance), 3)
-
-        # 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 dist
-
 class BatPercent(DigitalSensor):
     def __init__(self, *args, **kwargs):
         super(BatPercent, self).__init__(*args, **kwargs)

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

@@ -6,15 +6,16 @@
   Description:  A base controller class for the Arduino DUE microcontroller
   Author: corvin
   History:
-      20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
+    20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
         因为在arduino代码会根据超时自己会执行pwm清零停止电机移动的命令.
+    20210913:为增加两轮平衡车模式优化代码. 
 """
 import os
 import rospy
 import roslib; roslib.load_manifest('ros_arduino_python')
 import dynamic_reconfigure.client
 
-from math import sin, cos, pi, sqrt
+from math import sin, cos, pi
 from geometry_msgs.msg import Quaternion, Twist
 from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
@@ -24,16 +25,12 @@ from std_msgs.msg import Int32
 """ Class to receive Twist commands and publish wheel Odometry data """
 class BaseController:
 
-    def __init__(self, is_use_serial, arduino, base_frame, name="base_controllers_node"):
+    def __init__(self, is_use_serial, arduino, base_frame):
         self.use_serial = is_use_serial
         self.arduino    = arduino
-        self.name       = name
         self.base_frame = base_frame
         self.odom_name  = rospy.get_param("~odom_name", "odom")
         self.cmd_topic  = rospy.get_param("~cmd_topic", "cmd_vel")
-        self.rate       = float(rospy.get_param("~base_controller_rate", 20))
-        self.timeout    = rospy.get_param("~base_controller_timeout", 0.7)
-
         self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
 
         pid_params = dict()
@@ -51,7 +48,7 @@ class BaseController:
         pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
         pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 10)
 
-        self.accel_limit  = rospy.get_param('~accel_limit', 0.05)
+        self.accel_limit  = rospy.get_param('~accel_limit', 0.1)
         self.linear_scale_correction  = rospy.get_param("~linear_scale_correction",  1.0)
         self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
 
@@ -65,23 +62,18 @@ class BaseController:
         self.ticks_per_meter = self.ticks_per_meter/self.linear_scale_correction
 
         # What is the maximum acceleration we will tolerate when changing wheel speeds?
-        self.max_accel = self.accel_limit*self.ticks_per_meter/self.rate
-
-        # Track how often we get a bad encoder count (if any)
-        self.bad_encoder_count = 0
+        self.max_accel = self.accel_limit*self.ticks_per_meter
 
         now = rospy.Time.now()
-        self.then    = now  # time for determining dx/dy
-        self.t_delta = rospy.Duration(1.0/self.rate)
-        self.t_next  = now + self.t_delta
+        self.old_time = now  # time for determining dx/dy
 
-        # Internal data
-        self.enc_A = 0  # encoder readings
+        #用于保存读取的编码器计数值
+        self.enc_A = 0
         self.enc_B = 0
 
-        self.x  = 0  # position in xy plane
-        self.y  = 0
-        self.th = 0  # rotation in radians
+        self.x  = 0  #在平面上X轴方向位移
+        self.y  = 0  #在平面上Y轴方向位移
+        self.th = 0  #在平面上朝向
 
         self.v_A = 0
         self.v_B = 0
@@ -94,17 +86,17 @@ class BaseController:
         # Subscriptions main comtrol board to send control cmd
         rospy.Subscriber(self.cmd_topic, Twist, self.cmdVelCallback)
 
-        # Clear any old odometry info
+        #将编码器计数值清零
         if self.use_serial:
             self.arduino.reset_encoders()
         else:
             self.arduino.i2c_reset_encoders()
 
         # Set up the wheel odometry broadcaster
-        self.odomPub = rospy.Publisher(self.odom_name, Odometry, queue_size=10)
+        self.odomPub = rospy.Publisher(self.odom_name, Odometry, queue_size=5)
         self.odomBroadcaster = TransformBroadcaster()
 
-        #corvin add for rqt_plot debug pid
+        #Add for rqt_plot debug pid
         if self.debugPID:
             self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
             self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
@@ -114,7 +106,6 @@ class BaseController:
             self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
 
         rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m with " + str(self.encoder_resolution) + " ticks per rev")
-        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
 
     def setup_pid(self, pid_params):
         # Check to see if any PID parameters are missing
@@ -149,130 +140,96 @@ class BaseController:
             self.arduino.i2c_update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
                                         self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,)
 
-    # main loop, always run
+    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定
     def poll(self):
         self.send_debug_pid()
 
-        time_now = rospy.Time.now()
-        if time_now > self.t_next:
-            # Read the encoders
+        #读取编码器计数值,用于计算里程计信息
+        try:
+            if self.use_serial:
+                aWheel_enc, bWheel_enc = self.arduino.get_encoder_counts()
+            else:
+                aWheel_enc, bWheel_enc = self.arduino.i2c_get_encoder_counts()
+                rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
+        except:
             try:
-                if self.use_serial:
-                    aWheel_enc, bWheel_enc = self.arduino.get_encoder_counts()
-                else:
+                #rospy.logwarn("Failed to get encoder counts, try again. ")
+                if self.use_serial == False:
                     aWheel_enc, bWheel_enc = self.arduino.i2c_get_encoder_counts()
-                    #rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
             except:
-                try:
-                    # try again
-                    #rospy.logwarn("Failed to get encoder counts, try again. ")
-                    aWheel_enc, bWheel_enc = self.arduino.i2c_get_encoder_counts()
-                except:
-                    self.bad_encoder_count += 1
-                    rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
-                    return
-
-            #当读取到最新的编码器计数为0,说明下位机已经将计数清零，那上位机保存的上一次编码器计数也得清零
-            if aWheel_enc == 0:
-                self.enc_A = 0
-
-            if bWheel_enc == 0:
-                self.enc_B = 0
-
-            #根据编码器两次反馈的差值来计算车轮移动的距离
-            dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter
-            dist_B = (bWheel_enc - self.enc_B)/self.ticks_per_meter
-
-            # save current encoder value for next calculate
-            self.enc_A = aWheel_enc
-            self.enc_B = bWheel_enc
-
-            dt = time_now - self.then
-            self.then = time_now
-            dt = dt.to_sec()
-
-            va = dist_A/dt
-            vb = dist_B/dt
-
-            #forward kinemation
-            vx  = (-va + vb)/2.0
-            vy  = 0
-            vth = (-va - vb)/(self.wheel_track)
-
-            delta_x  = (vx*cos(self.th))*dt
-            delta_y  = (vx*sin(self.th))*dt
-            delta_th = vth*dt
-
-            self.x  += delta_x
-            self.y  += delta_y
-            self.th += delta_th
-
-            quaternion   = Quaternion()
-            quaternion.x = 0.0
-            quaternion.y = 0.0
-            quaternion.z = sin(self.th/2.0)
-            quaternion.w = cos(self.th/2.0)
-
-            odom = Odometry()
-            odom.header.frame_id = self.odom_name
-            odom.child_frame_id  = self.base_frame
-            odom.header.stamp    = time_now
-            odom.pose.pose.position.x  = self.x
-            odom.pose.pose.position.y  = self.y
-            odom.pose.pose.position.z  = 0
-            odom.pose.pose.orientation = quaternion
-            odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
-                                    0, 1e-3, 1e-9, 0, 0, 0,
-                                    0, 0, 1e6, 0, 0, 0,
-                                    0, 0, 0, 1e6, 0, 0,
-                                    0, 0, 0, 0, 1e6, 0,
-                                    0, 0, 0, 0, 0, 1e-9]
-            odom.twist.twist.linear.x  = vx
-            odom.twist.twist.linear.y  = vy
-            odom.twist.twist.angular.z = vth
-            odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
-                                    0, 1e-3, 1e-9, 0, 0, 0,
-                                    0, 0, 1e6, 0, 0, 0,
-                                    0, 0, 0, 1e6, 0, 0,
-                                    0, 0, 0, 0, 1e6, 0,
-                                    0, 0, 0, 0, 0, 1e-9]
-            self.odomPub.publish(odom)
-
-            # send motor cmd
-            if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
-                self.stop()
-            else:
-                self.send_motor_speed()
+                rospy.logerr("Encoder exception count !")
+                return
 
-            # set next compare time
-            self.t_next = time_now + self.t_delta
+        #当读取到最新的编码器计数为0,说明下位机已经将计数清零,那上位机保存的上一次编码器计数也得清零
+        if aWheel_enc == 0:
+            self.enc_A = 0
+        if bWheel_enc == 0:
+            self.enc_B = 0
 
+        #根据编码器两次反馈的差值来计算车轮移动的距离
+        dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter
+        dist_B = (bWheel_enc - self.enc_B)/self.ticks_per_meter
 
-    # debug motor pid parameter
-    def send_debug_pid(self):
-        if self.debugPID:
-            try:
-                if self.use_serial:
-                    A_pidin, B_pidin = self.arduino.get_pidin()
-                else:
-                    A_pidin, B_pidin = self.arduino.i2c_get_pidin()
-                self.AEncoderPub.publish(A_pidin)
-                self.BEncoderPub.publish(B_pidin)
-            except:
-                rospy.logerr("getpidin exception count:")
-                return
-
-            try:
-                if self.use_serial:
-                    A_pidout, B_pidout = self.arduino.get_pidout()
-                else:
-                    A_pidout, B_pidout = self.arduino.i2c_get_pidout()
-                self.APidoutPub.publish(A_pidout)
-                self.BPidoutPub.publish(B_pidout)
-            except:
-                rospy.logerr("getpidout exception count")
-                return
+        #保存当前编码器计数值用于下次计算
+        self.enc_A = aWheel_enc
+        self.enc_B = bWheel_enc
 
+        time_now = rospy.Time.now()
+        dt = time_now - self.old_time
+        self.old_time = time_now
+        dt = dt.to_sec()
+
+        va = dist_A/dt
+        vb = dist_B/dt
+
+        #正向运动学模型,计算里程计模型
+        vx  = (-va + vb)/2.0
+        vth = (-va - vb)/(self.wheel_track)
+
+        delta_x  = (vx*cos(self.th))*dt
+        delta_y  = (vx*sin(self.th))*dt
+        delta_th = vth*dt
+
+        self.x  += delta_x
+        self.y  += delta_y
+        self.th += delta_th
+
+        quaternion   = Quaternion()
+        quaternion.x = 0.0
+        quaternion.y = 0.0
+        quaternion.z = sin(self.th/2.0)
+        quaternion.w = cos(self.th/2.0)
+
+        odom = Odometry()
+        odom.header.frame_id = self.odom_name
+        odom.child_frame_id  = self.base_frame
+        odom.header.stamp    = time_now
+        odom.pose.pose.position.x  = self.x
+        odom.pose.pose.position.y  = self.y
+        odom.pose.pose.position.z  = 0
+        odom.pose.pose.orientation = quaternion
+        odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
+                                0, 1e-3, 1e-9, 0, 0, 0,
+                                0, 0, 1e6, 0, 0, 0,
+                                0, 0, 0, 1e6, 0, 0,
+                                0, 0, 0, 0, 1e6, 0,
+                                0, 0, 0, 0, 0, 1e-9]
+        odom.twist.twist.linear.x  = vx
+        odom.twist.twist.linear.y  = 0
+        odom.twist.twist.angular.z = vth
+        odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
+                                0, 1e-3, 1e-9, 0, 0, 0,
+                                0, 0, 1e6, 0, 0, 0,
+                                0, 0, 0, 1e6, 0, 0,
+                                0, 0, 0, 0, 1e6, 0,
+                                0, 0, 0, 0, 0, 1e-9]
+        self.odomPub.publish(odom)
+
+        #向电机发送控制命令,当前电机为运动状态,发送停止控制命令
+        if time_now < (self.last_cmd_time + rospy.Duration(0.1)):
+            self.send_motor_speed()
+            rospy.loginfo("send motor speed")
+            
     # normalize motor dest encode value and send
     def send_motor_speed(self):
         if self.v_A < self.v_des_AWheel:
@@ -293,7 +250,7 @@ class BaseController:
           if self.v_B < self.v_des_BWheel:
               self.v_B = self.v_des_BWheel
 
-        # send to arduino to drive motor
+        #向下位机发送控制电机转动的脉冲数
         if self.use_serial:
             self.arduino.drive(self.v_A, self.v_B)
         else:
@@ -303,27 +260,14 @@ class BaseController:
             self.AVelPub.publish(self.v_A)
             self.BVelPub.publish(self.v_B)
 
-    # stop move mobile base
-    def stop(self):
-        self.v_A = 0
-        self.v_B = 0
-        self.v_des_AWheel = 0
-        self.v_des_BWheel = 0
-        if self.use_serial:
-            self.arduino.drive(0, 0)
-        else:
-            self.arduino.i2c_drive(0, 0)
-        #rospy.logwarn("stop mobilebase move!!!!")
-
     def cmdVelCallback(self, req):
         # Handle velocity-based movement requests
         self.last_cmd_time = rospy.Time.now()
 
-        x  = req.linear.x  # m/s
-        y  = 0  # m/s
-        th = req.angular.z # rad/s
+        x  = req.linear.x  # x轴方向线速度　m/s
+        th = req.angular.z # z轴角速度　rad/s
 
-        #Inverse Kinematic
+        #两轮小车逆运动学模型,得到左右电机转速
         vA = -(x + 0.5*self.wheel_track*th)
         vB =  (x - 0.5*self.wheel_track*th)
 
@@ -331,3 +275,27 @@ class BaseController:
         self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)
         #rospy.loginfo("cmdCallback(): A v_des:"+str(self.v_des_AWheel)+",B v_des:"+str(self.v_des_BWheel))
 
+    #用于调试电机PID参数
+    def send_debug_pid(self):
+        if self.debugPID:
+            try:
+                if self.use_serial:
+                    A_pidin, B_pidin = self.arduino.get_pidin()
+                else:
+                    A_pidin, B_pidin = self.arduino.i2c_get_pidin()
+                self.AEncoderPub.publish(A_pidin)
+                self.BEncoderPub.publish(B_pidin)
+            except:
+                rospy.logerr("get pidin exception count !")
+                return
+
+            try:
+                if self.use_serial:
+                    A_pidout, B_pidout = self.arduino.get_pidout()
+                else:
+                    A_pidout, B_pidout = self.arduino.i2c_get_pidout()
+                self.APidoutPub.publish(A_pidout)
+                self.BPidoutPub.publish(B_pidout)
+            except:
+                rospy.logerr("get pidout exception count !")
+                return

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -4,7 +4,8 @@
 # Copyright: 2016-2021 wwww.corvin.cn ROS小课堂
 # Description: 通过键盘遥控小车行进，摄像头舵机转动。
 # History:
-#    20200726: 增加控制左右转动舵机的代码.
+#    20200726:增加控制左右转动舵机的代码.
+#    20210913:优化代码.
 
 from __future__ import print_function
 import roslib; roslib.load_manifest('teleop_twist_keyboard')
@@ -12,37 +13,33 @@ import rospy
 from geometry_msgs.msg import Twist
 import sys, select, termios, tty
 from ros_arduino_msgs.srv import CameraControl
-import time
 
 msg = """
 从键盘读取控制命令遥控小车移动,控制按键如下:
----------------------------
+------------------
    u    i    o
    j    k    l
    m    ,    .
-
-
-q/z : increase/decrease max speeds by 10%
-w/x : increase/decrease only linear speed by 10%
-e/c : increase/decrease only angular speed by 10%
-
+------------------
+q/z : 每次按照10%同比例来增加或减小线速度和角速度
+w/x : 每次按照10%比例来增加或减小线速度
+e/c : 每次按照10%比例来增加或减小角速度
 1/2 : 相机向上/向下转动
 3/4 : 相机向左/向右转动
 
-CTRL-C to quit
+Ctrl+c: 退出键盘控制模式 
 """
 
 moveBindings = {
-        'i':(1,0,0,0),
-        'j':(0,0,0,1),
-        'l':(0,0,0,-1),
-        ',':(-1,0,0,0),
-        'u':(1,0,0,1),
-        'o':(1,0,0,-1),
-        'm':(-1,0,0,-1),
-        '.':(-1,0,0,1),
+        'i':(1,0),
+        'j':(0,1),
+        'l':(0,-1),
+        ',':(-1,0),
+        'u':(1,1),
+        'o':(1,-1),
+        'm':(-1,-1),
+        '.':(-1,1),
     }
-
 speedBindings={
         'q':(1.1,1.1),
         'z':(.9,.9),
@@ -51,7 +48,6 @@ speedBindings={
         'e':(1,1.1),
         'c':(1,.9),
     }
-
 cameraUpBindings={
         '1':(-5),
     }
@@ -72,13 +68,11 @@ def getKey():
     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
     return key
 
-
-def vels(speed,turn):
-    return "当前控制速度:  线速度: %s  角速度: %s" % (speed,turn)
+def vels(speed, turn):
+    return "当前控制速度:  线速度: %s  角速度: %s" %(speed,turn)
 
 def angles(angle):
-    return "currently:\tangle %s " % (angle)
-
+    return "当前角度:  %s " % (angle)
 
 #调用service: mobilebase_arduino/camera_control控制相机上下转动
 def client_srv(ser_id, ser_angle):
@@ -86,22 +80,20 @@ def client_srv(ser_id, ser_angle):
     try:
         camera_client = rospy.ServiceProxy("mobilebase_arduino/camera_control", CameraControl)
         resp = camera_client.call(ser_id, ser_angle)
-    except rospy.ServiceException, e:
+    except (rospy.ServiceException, rospy.ROSException), e:
         rospy.logwarn("Service call failed: %s"%e)
 
-
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
-    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 2)
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
     rospy.init_node('teleop_twist_keyboard')
 
-    speed = rospy.get_param("~speed", 0.30)
-    turn = rospy.get_param("~turn", 1.0)
+    #初始化控制命令参数,线速度0.3m/s，角速度1.0rad/s
+    speed = 0.30
+    turn = 1.0
 
     x = 0
-    y = 0
-    z = 0
     th = 0
     status = 0
     upDownAngle = 90
@@ -109,26 +101,21 @@ if __name__=="__main__":
 
     try:
         print(msg)
-        print(vels(speed,turn))
+        print(vels(speed, turn))
         while(1):
             key = getKey()
             if key in moveBindings.keys():
                 x = moveBindings[key][0]
-                y = moveBindings[key][1]
-                z = moveBindings[key][2]
-                th = moveBindings[key][3]
+                th = moveBindings[key][1]
             elif key in speedBindings.keys():
                 speed = speed * speedBindings[key][0]
                 turn = turn * speedBindings[key][1]
-
                 print(vels(speed,turn))
-                if (status == 14):
+                if (status == 9):
                     print(msg)
-                status = (status + 1) % 15
+                status = (status + 1) % 10
             elif key in cameraUpBindings.keys():
                 x = 0
-                y = 0
-                z = 0
                 th = 0
                 print(angles(upDownAngle))
                 if (upDownAngle <= 180 and upDownAngle > 0):
@@ -136,8 +123,6 @@ if __name__=="__main__":
                     client_srv(1, upDownAngle)
             elif key in cameraDownBindings.keys():
                 x = 0
-                y = 0
-                z = 0
                 th = 0
                 print(angles(upDownAngle))
                 if (upDownAngle < 180 and upDownAngle >= 0):
@@ -145,8 +130,6 @@ if __name__=="__main__":
                     client_srv(1, upDownAngle)
             elif key in cameraLeftBindings.keys():
                 x = 0
-                y = 0
-                z = 0
                 th = 0
                 print(angles(leftRightAngle))
                 if (leftRightAngle < 180 and leftRightAngle >=0):
@@ -154,8 +137,6 @@ if __name__=="__main__":
                     client_srv(0, leftRightAngle)
             elif key in cameraRightBindings.keys():
                 x = 0
-                y = 0
-                z = 0
                 th = 0
                 print(angles(leftRightAngle))
                 if (leftRightAngle <= 180 and leftRightAngle >0):
@@ -163,20 +144,16 @@ if __name__=="__main__":
                     client_srv(0, leftRightAngle)
             else:
                 x = 0
-                y = 0
-                z = 0
                 th = 0
                 if (key == '\x03'):
                     break
 
             twist = Twist()
-            twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
+            twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0;
             twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
             pub.publish(twist)
-
     except Exception as e:
         print(e)
-
     finally:
         twist = Twist()
         twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
@@ -184,4 +161,3 @@ if __name__=="__main__":
         pub.publish(twist)
 
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
-