保持直线行走改用topic获取yaw数据

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/param.yaml

@@ -13,4 +13,4 @@ pub_temp_topic: imu_temp
 yaw_topic: yaw_data
 pub_yposition_topic: yposition_data
 link_name: base_imu_link
-pub_hz: 20
+pub_hz: 120

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -25,6 +25,7 @@ timeout: 0.5
 rate: 25
 
 cmd_topic: cmd_vel
+yaw_topic: yaw_data
 
 base_controller_rate: 10.0
 base_controller_timeout: 0.7
@@ -78,6 +79,6 @@ yaw_kd: 0.11
 
 # Forward Yaw Error and Rate
 correction_yaw_flag_timeout: 2
-correction_forward_err_yaw_data: 0.00
+correction_forward_err_yaw_data: 0.012
 correction_forward_rate: 1
 

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

@@ -22,6 +22,7 @@ from geometry_msgs.msg import Quaternion, Twist
 from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
 from std_msgs.msg import Int32
+from std_msgs.msg import Float32
 from rasp_imu_hat_6dof.srv import GetYawData
 
 class SimplePID:
@@ -95,6 +96,7 @@ class BaseController:
         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.yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
         self.rate       = float(rospy.get_param("~base_controller_rate", 20))
         self.timeout    = rospy.get_param("~base_controller_timeout", 0.7)
 
@@ -111,6 +113,7 @@ class BaseController:
         self.last_yaw_data = 0
         self.now_yaw_data = 0
         self.initial_odom_yaw_data = self.get_yaw()
+        self.yaw_topic_data = 0
 
         self.debugPID   = rospy.get_param('~debugPID', False)
 
@@ -181,6 +184,7 @@ class BaseController:
 
         # Subscriptions main comtrol board to send control cmd
         rospy.Subscriber(self.cmd_topic, Twist, self.cmdVelCallback)
+        rospy.Subscriber(self.yaw_topic_name,Float32,self.yawCallback)
 
         # Clear any old odometry info
         if self.use_serial:
@@ -321,7 +325,7 @@ class BaseController:
             self.pose_y += delta_y
             self.th += delta_th
 
-            self.now_odom_yaw_data = self.get_yaw()
+            self.now_odom_yaw_data = self.yaw_topic_data
             self.th = self.now_odom_yaw_data - self.initial_odom_yaw_data
 
             quaternion   = Quaternion()
@@ -482,11 +486,11 @@ class BaseController:
         if x != 0 and y == 0 and th == 0:
             if self.correction_yaw_flag < self.correction_yaw_flag_timeout:
                 self.stop()
-                self.last_yaw_data = self.get_yaw()
+                self.last_yaw_data = self.yaw_topic_data
                 self.correction_yaw_flag += 1
                 return
             else:
-                self.now_yaw_data = self.get_yaw()
+                self.now_yaw_data = self.yaw_topic_data
                 err_yaw_data = self.now_yaw_data - self.last_yaw_data
                 if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
                     th = self.pid_controller.update(err_yaw_data)
@@ -507,3 +511,5 @@ class BaseController:
         self.v_des_CWheel = int(vC * 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)+",C v_des:"+str(self.v_des_CWheel))
 
+    def yawCallback(self,req):
+        self.yaw_topic_data = req.data