运动学仿真曲线运动等更新

src/car_controller_pkg/car_controller_pkg/car_controller.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """
-双对角舵轮机器人曲线运动控制（精简验证版）
-仅实现：给定 v_x 和 omega_z，控制两轮转向角和速度
+双对角舵轮机器人曲线运动控制
+实现：给定 v_x/ v_x,v_y 和 omega_z，控制两轮转向角和速度
 """
 import rclpy
 from rclpy.node import Node
@@ -12,6 +12,7 @@ from rclpy.action import ActionClient
 import math
 import time
 
+
 class CurveMotionValidator(Node):
     def __init__(self):
         super().__init__('curve_motion_validator')
@@ -63,7 +64,7 @@ class CurveMotionValidator(Node):
             return False
 
         self.get_logger().info(
-            f"✅ 转向设置: RF={math.degrees(angle_rf):+.1f}°, LB={math.degrees(angle_lb):+.1f}°"
+            f"转向设置: RF={math.degrees(angle_rf):+.1f}°, LB={math.degrees(angle_lb):+.1f}°"
         )
         return True
 
@@ -72,7 +73,7 @@ class CurveMotionValidator(Node):
         msg = Float64MultiArray()
         msg.data = [speed_rf, speed_lb]
         self.drive_pub.publish(msg)
-        self.get_logger().info(f"⚙️ 驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}")
+        self.get_logger().info(f"驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}")
 
     def follow_curve(self, v_x, omega_z):
         """
@@ -80,35 +81,78 @@ class CurveMotionValidator(Node):
         :param v_x: 车体 x 方向线速度 (m/s)
         :param omega_z: 车体角速度 (rad/s)，正数左转，负数右转
         """
-        self.get_logger().info(f"🌀 开始曲线运动: v_x={v_x:.2f}, ω_z={omega_z:.2f}")
+        self.get_logger().info(f"开始曲线运动: v_x={v_x:.2f}, ω_z={omega_z:.2f}")
 
         # === 1. 计算转向角度 ===
-        # 右前轮 (RF): (L/2, W/2)
+        # 右前轮 (RF): (L/2, W/2) = (0.33, 0.145)
+        x_rf, y_rf = self.HALF_BASE, self.HALF_TRACK
+        theta_rf = math.atan2(omega_z * x_rf, v_x - omega_z * y_rf)
+
+        # 左后轮 (LB): (-L/2, -W/2) = (-0.33, -0.145)
+        x_lb, y_lb = -self.HALF_BASE, -self.HALF_TRACK
+        theta_lb = math.atan2(omega_z * x_lb, v_x - omega_z * y_lb)
+
+        # 设置转向
+        success = self.set_steer_angles(theta_rf, theta_lb, duration=0.3)
+        if not success:
+            return
+        time.sleep(0.5)  # 等待转向到位
+
+        # === 2. 计算驱动速度 ===
+        # 公式: v = v_x * cosθ + ω_z * (x*sinθ - y*cosθ)
+        def compute_wheel_speed(v_x, omega_z, x, y, steer_angle):
+            return v_x * math.cos(steer_angle) + omega_z * (x * math.sin(steer_angle) - y * math.cos(steer_angle))
+
+        v_rf = compute_wheel_speed(v_x, omega_z, x_rf, y_rf, theta_rf)
+        v_lb = compute_wheel_speed(v_x, omega_z, x_lb, y_lb, theta_lb)
+
+        # === 3. 转为电机角速度 ===
+        omega_rf = v_rf / self.WHEEL_RADIUS
+        omega_lb = v_lb / self.WHEEL_RADIUS
+
+        # 发送驱动速度
+        self.set_drive_speeds(omega_rf, omega_lb)
+
+    def follow_curve_all(self, v_x, v_y, omega_z):
+        """
+        核心函数：给定车体速度和角速度，控制全向运动
+        :param v_x: 车体 x 方向线速度 (m/s)
+        :param v_y: 车体 y 方向线速度 (m/s)
+        :param omega_z: 车体角速度 (rad/s)
+        """
+        self.get_logger().info(f"开始全向运动: v_x={v_x:.2f}, v_y={v_y:.2f}, ω_z={omega_z:.2f}")
+
+        # === 1. 计算转向角度（支持 v_y）===
+        x_rf, y_rf = self.HALF_BASE, self.HALF_TRACK
         theta_rf = math.atan2(
-            omega_z * self.HALF_BASE,
-            v_x - omega_z * self.HALF_TRACK
+            omega_z * x_rf + v_y,
+            v_x - omega_z * y_rf
         )
-        # 左后轮 (LB): (-L/2, -W/2)
+
+        x_lb, y_lb = -self.HALF_BASE, -self.HALF_TRACK
         theta_lb = math.atan2(
-            omega_z * (-self.HALF_BASE),
-            v_x - omega_z * (-self.HALF_TRACK)
+            omega_z * x_lb + v_y,
+            v_x - omega_z * y_lb
         )
 
         # 设置转向
-        self.set_steer_angles(theta_rf, theta_lb, duration=0.3)
+        success = self.set_steer_angles(theta_rf, theta_lb, duration=0.3)
+        if not success:
+            return
         time.sleep(0.5)  # 等待转向到位
 
-        # === 2. 计算轮子线速度大小 ===
-        v_rf = math.sqrt(
-            (v_x - omega_z * self.HALF_TRACK)**2 +
-            (omega_z * self.HALF_BASE)**2
-        )
-        v_lb = math.sqrt(
-            (v_x + omega_z * self.HALF_TRACK)**2 +
-            (omega_z * self.HALF_BASE)**2
-        )
+        # === 2. 计算驱动速度（支持 v_y）===
+        def compute_wheel_speed(v_x, v_y, omega_z, x, y, steer_angle):
+            return (
+                v_x * math.cos(steer_angle) +
+                v_y * math.sin(steer_angle) +
+                omega_z * (x * math.sin(steer_angle) - y * math.cos(steer_angle))
+            )
 
-        # === 3. 转为驱动电机角速度 ===
+        v_rf = compute_wheel_speed(v_x, v_y, omega_z, x_rf, y_rf, theta_rf)
+        v_lb = compute_wheel_speed(v_x, v_y, omega_z, x_lb, y_lb, theta_lb)
+
+        # === 3. 转为电机角速度 ===
         omega_rf = v_rf / self.WHEEL_RADIUS
         omega_lb = v_lb / self.WHEEL_RADIUS
 
@@ -124,16 +168,20 @@ def main():
     try:
         # === 验证曲线运动 ===
         # 示例1: 左弧线前进
-        controller.follow_curve(v_x=1.5, omega_z=0.8)
-        time.sleep(10)
+        # controller.follow_curve(v_x=1.5, omega_z=0.8)
+        # time.sleep(10)
 
         # 示例2: 右弧线前进
-        #controller.follow_curve(v_x=1.5, omega_z=-0.6)
-        #time.sleep(10)
+        # controller.follow_curve(v_x=1.5, omega_z=-0.6)
+        # time.sleep(10)
 
         # 示例3: 直行（omega_z=0）
-        #controller.follow_curve(v_x=2.0, omega_z=0.0)
-        #time.sleep(5)
+        # controller.follow_curve(v_x=2.0, omega_z=0.0)
+        # time.sleep(5)
+
+        # 示例4: 斜向转弯（全向运动）
+        controller.follow_curve_all(v_x=1.0, v_y=0.5, omega_z=0.5)
+        time.sleep(10)
 
         # 最终停止
         controller.set_drive_speeds(0.0, 0.0)

src/car_controller_pkg/car_controller_pkg/car_controller_line.py

@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+"""
+双对角舵轮机器人运动学控制脚本
+基于精确几何关系控制转向角，确保无滑动运动
+"""
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64MultiArray
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectoryPoint
+from rclpy.action import ActionClient
+import math
+import time
+
+class DualSwerveController(Node):
+    def __init__(self):
+        super().__init__('dual_swerve_controller')
+
+        # === 机器人参数（根据你的URDF调整）===
+        self.WHEEL_BASE = 0.66  # 轴距 L (前后距离)
+        self.WHEEL_TRACK = 0.29  # 轮距 W (左右距离)
+        self.HALF_BASE = self.WHEEL_BASE / 2.0  # 0.33
+        self.HALF_TRACK = self.WHEEL_TRACK / 2.0  # 0.145
+
+        # === 计算对角线夹角 ===
+        # 对角线与X轴夹角 alpha
+        self.alpha = math.atan2(self.HALF_TRACK, self.HALF_BASE)  # ≈ 23.7°
+        self.get_logger().info(f"对角线夹角 alpha = {math.degrees(self.alpha):.1f}°")
+
+        # === 发布器和客户端 ===
+        # 驱动速度发布
+        self.drive_pub = self.create_publisher(
+            Float64MultiArray,
+            '/drive_group_controller/commands',
+            10
+        )
+
+        # 转向控制器 Action 客户端
+        self.steer_client = ActionClient(
+            self,
+            FollowJointTrajectory,
+            '/steer_group_controller/follow_joint_trajectory'
+        )
+
+        # 等待控制器就绪
+        self.get_logger().info("等待转向控制器就绪...")
+        self.steer_client.wait_for_server()
+        self.get_logger().info("转向控制器已连接！")
+
+    def set_steer_angles(self, angle_rf, angle_lb, duration=1.0):
+        """
+        设置两个舵轮的转向角度（弧度）
+        angle_rf: 右前轮角度
+        angle_lb: 左后轮角度
+        """
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory.joint_names = ['Right_wheel_f_joint', 'Left_wheel_back_joint']
+        
+        point = JointTrajectoryPoint()
+        point.positions = [angle_rf, angle_lb]
+        point.time_from_start.sec = int(duration)
+        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
+        goal.trajectory.points = [point]
+
+        future = self.steer_client.send_goal_async(goal)
+        rclpy.spin_until_future_complete(self, future)
+        goal_handle = future.result()
+
+        if not goal_handle.accepted:
+            self.get_logger().error("转向目标被拒绝！")
+            return False
+
+        self.get_logger().info(f"转向角度已设置: RF={math.degrees(angle_rf):.1f}°, LB={math.degrees(angle_lb):.1f}°")
+        return True
+
+    def set_drive_speeds(self, speed_rf, speed_lb):
+        """
+        设置驱动轮速度（rad/s）
+        """
+        msg = Float64MultiArray()
+        msg.data = [speed_rf, speed_lb]
+        self.drive_pub.publish(msg)
+        self.get_logger().info(f"驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}")
+
+    def go_straight(self, speed=2.0):
+        """
+        直行：两舵轮平行于X轴
+        """
+        self.get_logger().info("执行：直行")
+        # 两轮都指向 X 正方向（0弧度）
+        self.set_steer_angles(0.0, 0.0, duration=1.0)
+        time.sleep(1.2)  # 等待转向到位
+        self.set_drive_speeds(speed, speed)
+
+    def rotate_in_place(self, speed=2.0, direction='left'):
+        """
+        原地旋转：两舵轮垂直于对角线
+        direction: 'left' 或 'right'
+        """
+        self.get_logger().info(f"执行：原地{direction}旋转")
+
+        # 方案1：工程实用法（错误）
+        # if direction == 'left':
+        #     self.set_steer_angles(math.pi/2, -math.pi/2, duration=1.0)
+        #     self.set_drive_speeds(speed, -speed)
+        # else:
+        #     self.set_steer_angles(-math.pi/2, math.pi/2, duration=1.0)
+        #     self.set_drive_speeds(speed, -speed)
+
+        # 方案2：精确几何法（已启用）
+        steer_angle = math.pi/2 - self.alpha  # ≈ 66.3°
+        if direction == 'left':
+            self.set_steer_angles(steer_angle, steer_angle, duration=1.0)
+        else:
+            self.set_steer_angles(-steer_angle, -steer_angle, duration=1.0)
+        self.set_drive_speeds(speed, -speed)
+
+    def turn_arc(self, speed=2.0, turn_angle=math.pi/2):
+        """
+        弧线转弯：两舵轮同向偏转
+        turn_angle: 弧度，正数为左转，负数为右转
+        """
+        direction = "左" if turn_angle > 0 else "右"
+        self.get_logger().info(f" 执行：{direction}转弯斜行")
+
+        self.set_steer_angles(turn_angle, turn_angle, duration=1.0)
+        time.sleep(1.2)
+        self.set_drive_speeds(speed, speed)
+
+    def stop(self):
+        """停止所有运动"""
+        self.get_logger().info("停止")
+        self.set_drive_speeds(0.0, 0.0)
+
+    def print_geometry_info(self):
+        """打印几何参数供调试"""
+        self.get_logger().info("   机器人几何参数:")
+        self.get_logger().info(f"  轴距 (L): {self.WHEEL_BASE:.3f} m")
+        self.get_logger().info(f"  轮距 (W): {self.WHEEL_TRACK:.3f} m")
+        self.get_logger().info(f"  对角线夹角 α: {math.degrees(self.alpha):.1f}°")
+        self.get_logger().info(f"  垂直角度 (90°-α): {math.degrees(math.pi/2 - self.alpha):.1f}°")
+
+
+def main():
+    rclpy.init()
+    controller = DualSwerveController()
+    
+    # 打印几何信息
+    controller.print_geometry_info()
+    time.sleep(1)
+
+    try:
+        # === 测试序列 ===
+        # 1. 直行 10秒
+        controller.go_straight(speed=2.0)
+        time.sleep(10)
+
+        # 2. 停止
+        controller.stop()
+        time.sleep(5)
+
+        # 3. 原地左转 5秒
+        controller.rotate_in_place(speed=2.0, direction='left')
+        time.sleep(20)
+
+        # 4. 停止
+        controller.stop()
+        time.sleep(5)
+
+        # 5. 右转弯 5秒
+        controller.turn_arc(speed=2.0, turn_angle=-0.5)
+        time.sleep(15)
+
+        # 6. 最终停止
+        controller.stop()
+
+    except Exception as e:
+        controller.get_logger().error(f"运行出错: {e}")
+    finally:
+        controller.stop()
+        controller.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()