first

src/car_controller_pkg/car_controller_pkg/car_controller.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+"""
+双对角舵轮机器人曲线运动控制（精简验证版）
+仅实现：给定 v_x 和 omega_z，控制两轮转向角和速度
+"""
+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 CurveMotionValidator(Node):
+    def __init__(self):
+        super().__init__('curve_motion_validator')
+
+        # === 机器人参数 ===
+        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
+
+        # 轮子半径（用于速度转换）
+        self.WHEEL_RADIUS = 0.1  # 米
+
+        # === 发布器和客户端 ===
+        self.drive_pub = self.create_publisher(
+            Float64MultiArray,
+            '/drive_group_controller/commands',
+            10
+        )
+
+        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=0.3):
+        """设置右前轮和左后轮的转向角度（弧度）"""
+        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 follow_curve(self, v_x, omega_z):
+        """
+        核心函数：给定车体速度和角速度，控制曲线运动
+        :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}")
+
+        # === 1. 计算转向角度 ===
+        # 右前轮 (RF): (L/2, W/2)
+        theta_rf = math.atan2(
+            omega_z * self.HALF_BASE,
+            v_x - omega_z * self.HALF_TRACK
+        )
+        # 左后轮 (LB): (-L/2, -W/2)
+        theta_lb = math.atan2(
+            omega_z * (-self.HALF_BASE),
+            v_x - omega_z * (-self.HALF_TRACK)
+        )
+
+        # 设置转向
+        self.set_steer_angles(theta_rf, theta_lb, duration=0.3)
+        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
+        )
+
+        # === 3. 转为驱动电机角速度 ===
+        omega_rf = v_rf / self.WHEEL_RADIUS
+        omega_lb = v_lb / self.WHEEL_RADIUS
+
+        # 发送驱动速度
+        self.set_drive_speeds(omega_rf, omega_lb)
+
+
+def main():
+    rclpy.init()
+    controller = CurveMotionValidator()
+    time.sleep(1)
+
+    try:
+        # === 验证曲线运动 ===
+        # 示例1: 左弧线前进
+        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)
+
+        # 示例3: 直行（omega_z=0）
+        #controller.follow_curve(v_x=2.0, omega_z=0.0)
+        #time.sleep(5)
+
+        # 最终停止
+        controller.set_drive_speeds(0.0, 0.0)
+
+    except Exception as e:
+        controller.get_logger().error(f"运行出错: {e}")
+    finally:
+        controller.set_drive_speeds(0.0, 0.0)
+        controller.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/car_controller_pkg/car_controller_pkg/car_controller（复件）.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()

src/car_controller_pkg/car_controller_pkg/send_zero_cmd.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64MultiArray
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy
+import time
+
+class ZeroCmdSender(Node):
+    def __init__(self):
+        super().__init__('zero_cmd_sender')
+        # 使用 TRANSIENT_LOCAL，确保消息被 late-joining 订阅者接收
+        qos = QoSProfile(depth=1)
+        qos.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
+
+        self.pub = self.create_publisher(
+            Float64MultiArray,
+            '/drive_group_controller/commands',
+            qos
+        )
+        self.get_logger().info("✅ ZeroCmdSender node started, preparing to send zero command.")
+
+    def send_zero_and_wait(self, max_wait=2.0, retry=3):
+        """
+        发送零速度命令，并等待至少一个订阅者连接
+        """
+        msg = Float64MultiArray()
+        msg.data = [0.0, 0.0]
+
+        # 等待订阅者连接（最多等待 max_wait 秒）
+        start_time = self.get_clock().now()
+        while (self.pub.get_subscription_count() == 0 and
+               (self.get_clock().now() - start_time).nanoseconds / 1e9 < max_wait):
+            self.get_logger().info("⏳ Waiting for subscriber on /drive_group_controller/commands...")
+            time.sleep(0.1)
+
+        # 无论是否等到，都尝试发送
+        for i in range(retry):
+            self.pub.publish(msg)
+            self.get_logger().info(f"✅ Published zero velocity command: [0.0, 0.0] (attempt {i+1})")
+            time.sleep(0.05)  # 小延迟，提高发送成功率
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ZeroCmdSender()
+    node.send_zero_and_wait()
+    time.sleep(0.5)  # 额外缓冲
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/car_controller_pkg/car_controller_pkg/test_wheel_vel.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+
+class CmdVelToWheels(Node):
+    def __init__(self):
+        super().__init__('cmd_vel_to_wheels')
+
+        # 参数（根据你的机器人调整）
+        self.declare_parameter('wheel_radius', 0.05)   # 轮子半径（米）
+        self.declare_parameter('wheel_separation', 0.3)  # 左右轮横向距离
+
+        self.wheel_radius = self.get_parameter('wheel_radius').value
+        self.wheel_separation = self.get_parameter('wheel_separation').value
+
+        # 发布到两个 velocity_controller
+        self.pub_front_left = self.create_publisher(Float64, '/front_left_controller/commands', 10)
+        self.pub_rear_right = self.create_publisher(Float64, '/rear_right_controller/commands', 10)
+
+        # 订阅 /cmd_vel
+        self.sub_cmd_vel = self.create_subscription(
+            Twist,
+            '/cmd_vel',
+            self.cmd_vel_callback,
+            10
+        )
+
+        self.get_logger().info("CmdVelToWheels bridge is running.")
+
+    def cmd_vel_callback(self, msg):
+        vx = msg.linear.x
+        wz = msg.angular.z
+
+        # 差速模型：计算左右轮线速度
+        left_vel = vx - wz * self.wheel_separation / 2
+        right_vel = vx + wz * self.wheel_separation / 2
+
+        # 转为角速度（rad/s）
+        front_left_omega = left_vel / self.wheel_radius
+        rear_right_omega = right_vel / self.wheel_radius
+
+        # 发布命令
+        self.pub_front_left.publish(Float64(data=front_left_omega))
+        self.pub_rear_right.publish(Float64(data=rear_right_omega))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CmdVelToWheels()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/car_controller_pkg/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>car_controller_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="15630071+llyg777@user.noreply.gitee.com">hx</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/car_controller_pkg/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/car_controller_pkg
+[install]
+install_scripts=$base/lib/car_controller_pkg

src/car_controller_pkg/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+
+package_name = 'car_controller_pkg'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='your_name',
+    maintainer_email='your_email@example.com',
+    description='Car controller package with path following and wheel control',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'car_controller = car_controller_pkg.car_controller:main',           # 路径控制
+            'cmd_vel_to_wheels = car_controller_pkg.test_wheel_vel:main',        # 测试节点
+            'send_zero_cmd = car_controller_pkg.send_zero_cmd:main',             # 置零节点
+
+        ],
+    },
+)

src/car_controller_pkg/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/car_controller_pkg/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/car_controller_pkg/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'