实现要求

src/four_wheel_robot/scripts/final_circle_controller.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+import rospy
+import math
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu
+import tf
+
+class FinalCircleController:
+    def __init__(self):
+        rospy.init_node('final_circle_controller')
+        
+        # 经过调整的控制器参数
+        self.target_radius = 1.5  # 减小半径，更容易观察
+        self.linear_speed = 0.5   # 降低速度，更稳定
+        self.center_x = 0.0
+        self.center_y = 1.5
+        
+        # 计算角速度
+        self.angular_speed = self.linear_speed / self.target_radius
+        
+        # 发布器
+        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+        
+        # 订阅器
+        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
+        self.imu_sub = rospy.Subscriber('/imu/data', Imu, self.imu_callback)
+        
+        self.last_odom_time = rospy.Time.now()
+        self.odom_count = 0
+        self.trajectory_points = []
+        
+        rospy.loginfo("=== 最终圆形轨迹控制器 ===")
+        rospy.loginfo(f"目标半径: {self.target_radius}米")
+        rospy.loginfo(f"圆心坐标: ({self.center_x}, {self.center_y})")
+        rospy.loginfo(f"线速度: {self.linear_speed}米/秒")
+        rospy.loginfo(f"角速度: {self.angular_speed:.3f}弧度/秒")
+        rospy.loginfo("等待Gazebo启动...")
+        
+    def odom_callback(self, msg):
+        # 每秒显示一次里程数据
+        current_time = rospy.Time.now()
+        if (current_time - self.last_odom_time).to_sec() >= 1.0:
+            x = msg.pose.pose.position.x
+            y = msg.pose.pose.position.y
+            
+            # 记录轨迹点
+            self.trajectory_points.append((x, y))
+            if len(self.trajectory_points) > 50:
+                self.trajectory_points.pop(0)
+            
+            # 计算到圆心的距离
+            distance_to_center = math.sqrt((x - self.center_x)**2 + (y - self.center_y)**2)
+            
+            # 获取朝向
+            orientation = msg.pose.pose.orientation
+            (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
+                [orientation.x, orientation.y, orientation.z, orientation.w])
+            
+            # 获取速度
+            vx = msg.twist.twist.linear.x
+            vy = msg.twist.twist.linear.y
+            actual_speed = math.sqrt(vx**2 + vy**2)
+            angular_speed = msg.twist.twist.angular.z
+            
+            # 计算轨迹误差
+            radius_error = abs(distance_to_center - self.target_radius)
+            
+            rospy.loginfo(f"里程[{self.odom_count:03d}]: "
+                         f"位置=({x:6.3f}, {y:6.3f}) | "
+                         f"半径误差={radius_error:5.3f}m | "
+                         f"速度={actual_speed:4.2f}m/s | "
+                         f"朝向={math.degrees(yaw):6.1f}°")
+            
+            # 每10秒显示轨迹质量
+            if self.odom_count % 10 == 0 and len(self.trajectory_points) > 10:
+                self.analyze_trajectory()
+            
+            self.last_odom_time = current_time
+            self.odom_count += 1
+    
+    def imu_callback(self, msg):
+        # 显示IMU数据（可选）
+        if self.odom_count % 20 == 0:  # 减少显示频率
+            accel = msg.linear_acceleration
+            gyro = msg.angular_velocity
+            rospy.loginfo(f"IMU - 加速度: [{accel.x:5.2f}, {accel.y:5.2f}, {accel.z:5.2f}] | "
+                         f"角速度: [{gyro.x:5.2f}, {gyro.y:5.2f}, {gyro.z:5.2f}]")
+    
+    def analyze_trajectory(self):
+        """分析轨迹质量"""
+        if len(self.trajectory_points) < 5:
+            return
+            
+        # 计算平均半径和标准差
+        radii = []
+        for x, y in self.trajectory_points:
+            radius = math.sqrt((x - self.center_x)**2 + (y - self.center_y)**2)
+            radii.append(radius)
+        
+        avg_radius = sum(radii) / len(radii)
+        radius_std = math.sqrt(sum((r - avg_radius)**2 for r in radii) / len(radii))
+        
+        rospy.loginfo(f"轨迹分析: 平均半径={avg_radius:.3f}m, 标准差={radius_std:.3f}m")
+        
+        if radius_std < 0.05:
+            rospy.loginfo("✅ 轨迹质量: 优秀 - 圆形轨迹稳定")
+        elif radius_std < 0.1:
+            rospy.loginfo("⚠️  轨迹质量: 良好 - 轻微波动")
+        elif radius_std < 0.2:
+            rospy.loginfo("❌ 轨迹质量: 一般 - 需要调整参数")
+        else:
+            rospy.loginfo("🚫 轨迹质量: 差 - 参数需要重大调整")
+    
+    def run(self):
+        # 等待Gazebo完全启动
+        rospy.sleep(3.0)
+        rospy.loginfo("开始发布控制命令...")
+        
+        rate = rospy.Rate(20)  # 20Hz控制频率
+        
+        while not rospy.is_shutdown():
+            try:
+                twist_msg = Twist()
+                twist_msg.linear.x = self.linear_speed
+                twist_msg.angular.z = self.angular_speed
+                
+                self.cmd_pub.publish(twist_msg)
+                rate.sleep()
+                
+            except Exception as e:
+                rospy.logerr(f"控制循环错误: {e}")
+                break
+
+if __name__ == '__main__':
+    try:
+        controller = FinalCircleController()
+        controller.run()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("控制器正常退出")
+    except Exception as e:
+        rospy.logerr(f"控制器启动错误: {e}")