将teleop_twist_keyboard软件包更名为control_robot,防止更默认的包重名

src/teleop_twist_keyboard/CMakeLists.txt → src/control_robot/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(teleop_twist_keyboard)
+project(control_robot)
 
 find_package(catkin REQUIRED)
 
@@ -8,7 +8,7 @@ catkin_package()
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-   teleop_twist_keyboard.py
+   control_robot.py
    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

src/teleop_twist_keyboard/teleop_twist_keyboard.py → src/control_robot/control_robot.py

@@ -1,22 +1,23 @@
 #!/usr/bin/env python
 # coding:utf-8
 
-# Copyright: 2016-2021 wwww.corvin.cn ROS小课堂
+# Copyright: 2016-2022 wwww.corvin.cn ROS小课堂
 # Description: 通过键盘遥控小车行进，摄像头舵机转动,2DOF机械臂转动.
 # History:
 #    20200726:增加控制左右转动舵机的代码.
 #    20210913:优化代码.
 #    20211117:将控制舵机转动的代码进行整合优化,精简代码.
+#    20220224:更新软件包名,代码文件名.
 
 from __future__ import print_function
-import rospy, roslib; roslib.load_manifest('teleop_twist_keyboard')
+import rospy, roslib
+import sys,select,termios,tty
 from geometry_msgs.msg import Twist
-import sys, select, termios, tty
 from ros_arduino_msgs.srv import ServoWrite
 
 msg = """
-从键盘读取控制命令遥控小车移动,控制按键如下:
------------------------------
+使用键盘控制小车的夹爪和移动,控制按键如下:
+------------------------------
      (左前) (前进) (右前)
        u      i      o
 (左转) j      k      l (右转)
@@ -96,7 +97,7 @@ if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
     pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
-    rospy.init_node('teleop_twist_keyboard')
+    rospy.init_node('control_robot_node')
 
     #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s
     speed = 0.2
@@ -109,7 +110,7 @@ if __name__=="__main__":
     cameraLeftRightAngle  = 90
     gripperUpDownAngle    = 90
     gripperOpenCloseAngle = 75
-    
+
     try:
         print(msg)
         print(vels(speed, turn))
@@ -121,7 +122,7 @@ if __name__=="__main__":
             status = (status + 1) % 20
             if (status == 0):
                 print(msg)
-                
+
             if key in moveBindings.keys():
                 x = moveBindings[key][0]
                 th = moveBindings[key][1]
@@ -173,7 +174,7 @@ if __name__=="__main__":
             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:
@@ -183,3 +184,4 @@ if __name__=="__main__":
         pub.publish(twist)
 
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+

src/teleop_twist_keyboard/package.xml → src/control_robot/package.xml

@@ -1,17 +1,11 @@
 <?xml version="1.0"?>
 <package>
-  <name>teleop_twist_keyboard</name>
+  <name>control_robot</name>
   <version>0.6.2</version>
   <description>Generic keyboard teleop for twist robots.</description>
-
-  <maintainer email="namniart@gmail.com">Austin Hendrix</maintainer>
-
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
   <license>BSD</license>
 
-  <url type="website">http://wiki.ros.org/teleop_twist_keyboard</url>
-
-  <author>Graylin Trevor Jay</author>
-
   <buildtool_depend>catkin</buildtool_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>rospy</run_depend>

src/robot_bringup/CMakeLists.txt

@@ -12,83 +12,6 @@ find_package(catkin REQUIRED)
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 ###################################
 ## catkin specific configuration ##
 ###################################

src/robot_bringup/launch/odom_ekf.py

@@ -6,15 +6,15 @@
     type nav_msgs/Odometry so we can view it in RViz.
 """
 import rospy
-from geometry_msgs.msg import PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseWithCovarianceStamped
 
 class OdomEKF():
     def __init__(self):
         rospy.init_node('odom_ekf_node', anonymous=False)
 
         # Publisher of type nav_msgs/Odometry
-        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
 
         # Wait for the /odom_combined topic to become available
         rospy.wait_for_message('input', PoseWithCovarianceStamped)
@@ -38,3 +38,4 @@ if __name__ == '__main__':
         rospy.spin()
     except:
         pass
+

src/robot_bringup/launch/robot_bringup.launch

@@ -1,6 +1,6 @@
 <!--
-  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
-  Description:robot启动时最先需要启动的launch文件，主要加载各基础功能模块启动。
+  Copyright: 2016-2022 ROS小课堂 www.corvin.cn
+  Description:robot启动时最先需要启动的launch文件,主要加载各基础功能模块启动.
     首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
     启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
     启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计

src/robot_bringup/package.xml

@@ -7,53 +7,15 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="corvin@todo.todo">corvin</maintainer>
-
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
 
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/robot_bringup</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-
-
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

src/ros_arduino_bridge/ros_arduino_python/CMakeLists.txt

@@ -8,19 +8,15 @@ catkin_python_setup()
 generate_dynamic_reconfigure_options(cfg/ROSArduinoBridge.cfg)
 
 catkin_package(DEPENDS)
-
 catkin_package(CATKIN_DEPENDS dynamic_reconfigure)
 
 install(DIRECTORY config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-
 install(DIRECTORY launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-
 install(DIRECTORY nodes
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
-

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -1,8 +1,7 @@
-# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Copyright: 2016-2022 www.corvin.cn ROS小课堂
 # Description:arduino节点运行时加载的参数,下面对参数进行解释:
 #   is_use_serial:与arduino通信是否使用串口,false的话就是IIC通信
 #   serial_port:串口通信时arduino的端口号
-#   serial_baud:串口通信时波特率
 #   i2c_smbus_num:系统管理总线号,树莓派为1
 #   isc_slave_addr:arduino的IIC设备地址
 #   default_main_rate:默认主循环的频率,不在平衡小车模式下
@@ -12,12 +11,12 @@
 #   20200706:init this file.
 #   20211016:增加两轮小车平衡模式下的参数.
 #   20211116:增加默认主循环和平衡小车模式下两个不同配置参数.
+#   20220224:删除通信波特率配置参数,因为是固定无需修改.
 
 is_use_serial: True
 
 #连接arduino设备的串口号和波特率
 serial_port: /dev/ttyACM0
-serial_baud: 57600
 
 i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
 i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
@@ -69,3 +68,4 @@ sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5},
   batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1},
 }
+

src/ros_arduino_bridge/ros_arduino_python/launch/arduino.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2022 ROS小课堂 www.corvin.cn
   Description:
     启动底盘arduino的launch文件,负责与arduino建立通信,发送获取信息.
   Author: corvin

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -2,14 +2,14 @@
 #_*_ coding:utf-8 _*_
 
 """
-  Copyright:2016-2021 www.corvin.cn ROS小课堂
+  Copyright:2016-2022 www.corvin.cn ROS小课堂
   Description: A ROS Node for the Arduino DUE microcontroller
   Author: corvin
   History:
     20200706:init this file.
+    20220224:优化精简代码,使可读性更好.
 """
-import rospy
-import os, thread
+import rospy, os, thread
 from ros_arduino_msgs.srv import *
 from geometry_msgs.msg import Twist
 from serial.serialutil import SerialException
@@ -23,7 +23,6 @@ class ArduinoROS():
 
         self.is_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.base_frame       = rospy.get_param("~base_frame", 'base_footprint')
@@ -59,7 +58,7 @@ class ArduinoROS():
 
         #用来获取红外测据传感器距离信息的服务
         rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
-        
+
         #设置指定舵机转动到指定角度的服务
         rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
 
@@ -67,7 +66,7 @@ class ArduinoROS():
         rospy.on_shutdown(self.shutdown)
 
         # Initialize the controlller
-        self.controller = Arduino(self.is_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr)
+        self.controller = Arduino(self.is_serial, self.serial_port, self.i2c_smbus_num, self.i2c_slave_addr)
 
         #与下位机建立连接
         self.controller.connect()
@@ -165,3 +164,4 @@ if __name__ == '__main__':
     except Exception:
         rospy.logerr("Get other unknown error !")
         os._exit(2)
+

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

@@ -2,7 +2,7 @@
 #-*- coding:utf-8 -*-
 
 """
-  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2022 ROS小课堂 www.corvin.cn
   Description: A Python driver for the Arduino microcontroller.
   Author: corvin, jally
   History:
@@ -17,10 +17,9 @@ import sys, traceback
 
 class Arduino:
 
-    def __init__(self, is_use_serial, serial_port, baudrate, i2c_smbus_num, i2c_slave_addr):
+    def __init__(self, is_use_serial, serial_port, i2c_smbus_num, i2c_slave_addr):
         self.is_use_serial = is_use_serial  #与下位机arduino的通信方式是否使用串口还是IIC
         self.serial_port   = serial_port
-        self.baudrate      = baudrate
 
         self.i2c_smbus_num  = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
@@ -43,13 +42,13 @@ class Arduino:
     #使用串口建立连接,进行通信
     def serial_connect(self):
         try:
-            self.serial_port = Serial(port=self.serial_port,baudrate=self.baudrate,timeout=self.timeout,writeTimeout=self.writeTimeout)
+            self.serial_port = Serial(port=self.serial_port,baudrate=57600,timeout=self.timeout,writeTimeout=self.writeTimeout)
             time.sleep(1)
             test = self.get_baud()
-            if test != self.baudrate:
+            if test != 57600:
                 time.sleep(1)
                 test = self.get_baud()
-                if test != self.baudrate:
+                if test != 57600:
                     raise SerialException
             self.beep_ring(1)  #连接串口成功的提示音
         except SerialException:
@@ -63,18 +62,10 @@ class Arduino:
         try:
             rospy.loginfo("Connecting to Arduino on IIC addr:"+str(self.i2c_slave_addr))
             test = self.i2c_get_baud()
-            #test2 = self.getCurrentValue()
-            #test3 = self.getBatPercent()
-            #test4 = self.detect_distance()
-            #test5 = self.analog_read(1)
-            #test6 = self.digital_read(10)
-            #test7 = self.i2c_get_pidin()
-            #test8 = self.i2c_get_encoder_counts()
-            #print test8
-            if test != self.baudrate:
+            if test != 57600:
                 time.sleep(1)
                 test = self.i2c_get_baud()
-                if test != self.baudrate:
+                if test != 57600:
                     raise Exception
 
             self.beep_ring(2)
@@ -101,16 +92,10 @@ class Arduino:
             self.i2c_bus.close()
 
     def send(self, cmd):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
         self.serial_port.write(cmd + '\r')
 
     def recv(self, timeout=0.5):
         timeout = min(timeout, self.timeout)
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner
-        '''
         c = ''
         value = ''
         attempts = 0
@@ -126,16 +111,10 @@ class Arduino:
         return value
 
     def recv_ack(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
         ack = self.recv(self.timeout)
         return ack == 'OK'
 
     def recv_int(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
         value = self.recv(self.timeout)
         try:
             return int(value)
@@ -143,9 +122,6 @@ class Arduino:
             return None
 
     def recv_array(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
         try:
             values = self.recv(self.timeout).split()
             return map(float, values)
@@ -153,8 +129,6 @@ class Arduino:
             return []
 
     def execute(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
-        '''
         self.mutex.acquire()
 
         try:

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

@@ -7,14 +7,12 @@
         20200330:增加获取电池剩余电量函数.
         20200423:增加获取电流函数
 """
-import roslib; roslib.load_manifest('ros_arduino_python')
-import rospy
+import roslib,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
 
@@ -43,7 +41,6 @@ class Sensor(object):
         self.t_delta = rospy.Duration(1.0 / self.rate)
         self.t_next = rospy.Time.now() + self.t_delta
 
-
     def poll(self):
         now = rospy.Time.now()
         if now > self.t_next:

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/balance_mode.py

@@ -2,15 +2,13 @@
 #-*- coding:utf-8 -*-
 
 """
-  Copyright: 2016-2021 www.corvin.cn ROS小课堂
+  Copyright: 2016-2022 www.corvin.cn ROS小课堂
   Description:  使两轮小车保持平衡模式,可以去掉支撑轮直立保持平衡和移动.
   Author: corvin
   History:
     20211005:初始化代码.
 """
-
-import os,rospy
-import roslib; roslib.load_manifest('ros_arduino_python')
+import os,rospy,roslib
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Imu
 
@@ -53,7 +51,7 @@ class BalanceMode:
 
     #平衡小车速度环,PI控制器
     def keepVelocity(self, encoder_A, encoder_B, target_speed):
-        encoder_err = encoder_A + encoder_B 
+        encoder_err = encoder_A + encoder_B
 
         #电机编码器计数进行一阶低通滤波
         self.encoder_lowOut = (1-self.encoderFilterPer)*encoder_err + \
@@ -66,7 +64,7 @@ class BalanceMode:
             self.encoder_integral = 1000
         elif self.encoder_integral < -1000:
             self.encoder_integral = -1000
-        
+
         #计算控制量pwm
         motorPWM = self.speed_kp*self.encoder_lowOut + self.speed_ki*self.encoder_integral
         #rospy.loginfo("integral="+str(self.encoder_integral)+" pwm="+str(motorPWM))
@@ -77,4 +75,4 @@ class BalanceMode:
         if pitch_err < self.downPitchData and pitch_err > (-self.downPitchData):
             return pwm_A, pwm_B
         else:
-            return 0, 0   
+            return 0, 0

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

@@ -2,17 +2,15 @@
 #-*- coding:utf-8 -*-
 
 """
-  Copyright: 2016-2021 www.corvin.cn ROS小课堂
+  Copyright: 2016-2022 www.corvin.cn ROS小课堂
   Description:  A base controller class for the Arduino DUE microcontroller
   Author: corvin
   History:
     20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
         因为在arduino代码会根据超时自己会执行pwm清零停止电机移动的命令.
-    20210913:为增加两轮平衡车模式优化代码. 
+    20210913:为增加两轮平衡车模式优化代码.
 """
-import os
-import rospy
-import roslib; roslib.load_manifest('ros_arduino_python')
+import os,rospy,roslib
 import dynamic_reconfigure.client
 
 from math import sin, cos, pi
@@ -32,10 +30,10 @@ class BaseController:
         self.cmd_topic  = cmd_topic
         self.PID_rate   = 100  #下位机中pid固定频率100hz
         self.target_speed = 0
-        
+
         self.odom_name  = rospy.get_param("~odom_name", "odom")
         self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
-        
+
         pid_params = dict()
         pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.067)
         pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
@@ -103,12 +101,12 @@ class BaseController:
 
         #增加用于调试PID参数的发布者
         if self.debugPID:
-            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
-            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
-            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=10)
-            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=10)
-            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=10)
-            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
+            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=5)
+            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=5)
+            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=5)
+            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=5)
+            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=5)
+            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=5)
 
         rospy.loginfo("Started base controller of " + str(self.wheel_track) + "m with " + str(self.encoder_resolution) + " ticks per rev")
 
@@ -207,7 +205,7 @@ class BaseController:
             #启动直立环控制,PD控制器,控制pitch角度和角速度
             stand_pwm = self.balanceControl.keepBalance(pitch_err)
             #stand_pwm = 0
-            
+
             #启动速度环控制,PI控制器
             #speed_pwm = self.balanceControl.keepVelocity(encoder_diff_A, encoder_diff_B, self.target_speed)
             speed_pwm = 0
@@ -218,11 +216,11 @@ class BaseController:
             #计算最终发送给电机的pwm
             pwm_A_tmp = stand_pwm + speed_pwm - turn_pwm
             pwm_B_tmp = stand_pwm + speed_pwm + turn_pwm
-            
+
             #将计算得到的pwm值进行限幅,最大pwm值为255,保持正负号
             pwm_A, pwm_B = self.pwm_ctrl(pwm_A_tmp, pwm_B_tmp)
             pwm_A, pwm_B = self.balanceControl.checkIfDown(pitch_err, pwm_A, pwm_B)
-            
+
             if self.use_serial:
                 self.arduino.pwm_drive(pwm_A, pwm_B)
             else:
@@ -287,7 +285,7 @@ class BaseController:
         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: