从下位机获取编码器计数为0的时候,需要也将上次保存的编码器计数清零,可以解决odom信息错误的问题

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -36,7 +36,7 @@ odom_name: odom
 wheel_diameter: 0.058
 wheel_track: 0.109     #L value
 encoder_resolution: 11 #12V DC motors
-gear_reduction: 103     #empty payload rpm 60 r/m
+gear_reduction: 103    #empty payload rpm 60 r/m
 linear_scale_correction: 1.028
 angular_scale_correction: 1.00
 

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -3,7 +3,7 @@
 
 """
   Copyright:2016-2020 www.corvin.cn ROS小课堂
-  Description: A ROS Node for the Arduino microcontroller
+  Description: A ROS Node for the Arduino DUE microcontroller
   Author: corvin
   History:
     20200412:init this file.
@@ -45,7 +45,7 @@ class ArduinoROS():
         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)
+        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)
         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
@@ -183,7 +183,7 @@ class ArduinoROS():
                                    req.value5, req.value6, 0, 50 )
         return DynamicPIDResponse()
 
-    # Stop the robot
+    # Stop the robot arduino connect
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")
         try:

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

@@ -50,6 +50,7 @@ class Arduino:
         # An array to cache digital sensor readings
         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
 
+    #与下位机arduino建立连接
     def connect(self):
         if self.is_use_serial:
             self.serial_connect()
@@ -57,6 +58,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)
@@ -78,6 +80,7 @@ class Arduino:
             rospy.logerr(traceback.print_exc(file=sys.stdout))
             os._exit(1)
 
+    #使用IIC总线进行数据通信
     def i2c_connect(self):
         try:
             rospy.loginfo("Connecting to Arduino on IIC addr:"+str(self.i2c_slave_addr))
@@ -417,6 +420,7 @@ class Arduino:
         except:
             return None
 
+    #使用串口发送停止电机转动命令
     def stop(self):
         ''' Stop all three motors.
         '''

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

@@ -20,6 +20,7 @@ from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
 from std_msgs.msg import Int32
 
+
 """ Class to receive Twist commands and publish wheel Odometry data """
 class BaseController:
 
@@ -88,10 +89,12 @@ class BaseController:
         self.pose_y  = 0
         self.th = 0  # rotation in radians
 
+        #存储三个车轮的当前转动速度
         self.v_A = 0
         self.v_B = 0
         self.v_C = 0
 
+        #存储需要控制三个车轮在PID周期内控制电机转动的脉冲数
         self.v_des_AWheel = 0  # cmd_vel setpoint
         self.v_des_BWheel = 0
         self.v_des_CWheel = 0
@@ -108,23 +111,23 @@ class BaseController:
             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
         if self.debugPID:
-            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=10)
-            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=10)
-            self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=10)
-            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=10)
-            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=10)
-            self.CPidoutPub  = rospy.Publisher('Cpidout',  Int32, queue_size=10)
-            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=10)
-            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
-            self.CVelPub     = rospy.Publisher('Cvel',     Int32, queue_size=10)
+            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=5)
+            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=5)
+            self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=5)
+            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=5)
+            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=5)
+            self.CPidoutPub  = rospy.Publisher('Cpidout',  Int32, queue_size=5)
+            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=5)
+            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=5)
+            self.CVelPub     = rospy.Publisher('Cvel',     Int32, queue_size=5)
 
         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")
+        rospy.loginfo("Publishing odometry 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
@@ -171,15 +174,16 @@ class BaseController:
     def poll(self):
         self.send_debug_pid()
 
-        time_now = rospy.Time.now()
+        time_now = rospy.Time.now() #获取当前系统时间
         if time_now > self.t_next:
-            # Read the encoders
+            # 读取三个电机反馈的编码器计数
             try:
                 if self.use_serial:
                     aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()
+                    #rospy.loginfo("get new encoder: "+str(aWheel_enc)+ " "+str(bWheel_enc)+" "+str(cWheel_enc))
                 else:
                     aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
-                    #rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
+                    #rospy.loginfo("get encoder counts: "++str(aWheel_enc)+ " "+str(bWheel_enc)+" "+str(cWheel_enc))
             except:
                 try:
                     # try again
@@ -190,33 +194,51 @@ class BaseController:
                     rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
                     return
 
-            # Calculate odometry
+            #当读取到最新的编码器计数为0,说明下位机已经将计数清零，那上位机保存的上一次编码器计数也得清零
+            if aWheel_enc == 0:
+                self.enc_A = 0
+
+            if bWheel_enc == 0:
+                self.enc_B = 0
+
+            if cWheel_enc == 0:
+                self.enc_C = 0
+
+            #根据编码器两次反馈的差值来计算车轮移动的距离
             dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter
             dist_B = (bWheel_enc - self.enc_B)/self.ticks_per_meter
             dist_C = (cWheel_enc - self.enc_C)/self.ticks_per_meter
+            #rospy.loginfo("get delta dist:"+str(dist_A)+" "+str(dist_B)+" "+str(dist_C))
 
-            # save current encoder value for next calculate
+            #将当前读取到的编码器计数保存,为了下次计算新脉冲数准备
             self.enc_A = aWheel_enc
             self.enc_B = bWheel_enc
             self.enc_C = cWheel_enc
+            #rospy.loginfo("get save encoder:"+str(self.enc_A)+" "+str(self.enc_B)+" "+str(self.enc_C))
 
-            dt = time_now - self.then
-            self.then = time_now
-            dt = dt.to_sec()
+            #计算两次获取编码器反馈的时间差
+            delta_time = time_now - self.then
+            delta_time = delta_time.to_sec()
 
-            va = dist_A/dt
-            vb = dist_B/dt
-            vc = dist_C/dt
+            #计算三个车轮的转动线速度
+            va = dist_A/delta_time
+            vb = dist_B/delta_time
+            vc = dist_C/delta_time
+            self.then = time_now
+            #rospy.loginfo("get vel:"+str(va)+ " "+str(vb)+" "+str(vc)+" delta_time:"+str(delta_time))
 
-            #forward kinemation
+            #正向运动学模型:根据三个车轮的速度合成小车整体的移动速度
             vx  = sqrt(3)*(va - vb)/3.0
             vy  = (va + vb - 2*vc)/3.0
             vth = (va + vb + vc)/(3*self.wheel_track)
 
-            delta_x  = (vx*cos(self.th) - vy*sin(self.th))*dt
-            delta_y  = (vx*sin(self.th) + vy*cos(self.th))*dt
-            delta_th = vth*dt
+            #计算移动的距离
+            delta_x  = (vx*cos(self.th) - vy*sin(self.th))*delta_time
+            delta_y  = (vx*sin(self.th) + vy*cos(self.th))*delta_time
+            delta_th = vth*delta_time
+            #rospy.loginfo("get delta pose:"+str(delta_x)+ " "+str(delta_y)+" "+str(delta_th))
 
+            #将移动的距离累加运算
             self.pose_x += delta_x
             self.pose_y += delta_y
             self.th += delta_th
@@ -338,11 +360,7 @@ class BaseController:
         self.v_des_AWheel = 0
         self.v_des_BWheel = 0
         self.v_des_CWheel = 0
-        #if self.use_serial:
-        #    self.arduino.drive(0, 0, 0)
-        #else:
-        #    self.arduino.i2c_drive(0, 0, 0)
-        #rospy.logwarn("stop mobilebase move!!!!")
+
 
     def cmdVelCallback(self, req):
         # Handle velocity-based movement requests