first

src/README.md

@@ -0,0 +1,2 @@
+# ros2_diff_car
+The differential car control simulation was carried out in Gazebo of ROS2

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'

src/car_description/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.8)
+project(car_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(urdf REQUIRED)
+find_package(xacro REQUIRED)
+
+install(
+  DIRECTORY
+    launch
+    urdf
+    config
+    scripts
+    rviz
+    meshes
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
+
+install(PROGRAMS
+  scripts/move.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/car_description/config/controllers.yaml

@@ -0,0 +1,52 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    # ✅ 驱动轮速度控制（两个对角轮）
+    drive_group_controller:
+      type: "velocity_controllers/JointGroupVelocityController"
+
+    # ✅ 转向轮位置控制（两个对角轮的转向）
+    steer_group_controller:
+      type: "joint_trajectory_controller/JointTrajectoryController"
+
+    # ✅ 发布关节状态
+    joint_state_broadcaster:
+      type: "joint_state_broadcaster/JointStateBroadcaster"
+
+# ========================
+# 驱动控制器：控制两个对角轮的速度
+# ========================
+drive_group_controller:
+  ros__parameters:
+    joints:
+      - Right_wheel_g_joint
+      - Left_wheel_back_g_joint
+    command_interfaces:
+      - velocity
+    state_interfaces:
+      - velocity
+      - position
+    state_publish_rate: 50.0
+# ========================
+# 转向控制器：控制两个对角轮的转向角度
+# ========================
+steer_group_controller:
+  ros__parameters:
+    joints:
+      - Right_wheel_f_joint          # 右前轮转向
+      - Left_wheel_back_joint        # 左后轮转向
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+
+# ========================
+# 关节状态发布
+# ========================
+joint_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 50.0

src/car_description/config/hardware_gazebo.yaml

@@ -0,0 +1,22 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    # 硬件系统定义
+    GazeboSystem:
+      type: system
+      hardware:
+        plugin: gazebo_ros2_control/GazeboSystem
+        # 可选参数
+        # param: 
+        #   sim_time_factor: 1.0
+
+    # 控制器列表
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    front_left_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    rear_right_controller:
+      type: velocity_controllers/JointGroupVelocityController

src/car_description/launch/car.launch.py

@@ -0,0 +1,58 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, Command
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue  # ✅ 新增导入
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    pkg_path = get_package_share_directory('car_description')
+    xacro_file = os.path.join(pkg_path, 'urdf', 'car.urdf.xacro')
+    rviz_config_file = os.path.join(pkg_path, 'rviz', 'urdf.rviz')
+
+    # 使用 Command 动态解析 Xacro，并用 ParameterValue 包装
+    robot_description = Command(['xacro ', xacro_file, ' use_sim_time:=', use_sim_time])
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[
+            {
+                'robot_description': ParameterValue(robot_description, value_type=str),  # ✅ 修复点
+                'use_sim_time': use_sim_time
+            }
+        ]
+    )
+
+    joint_state_publisher_gui = Node(
+        package='joint_state_publisher_gui',
+        executable='joint_state_publisher_gui',
+        name='joint_state_publisher_gui',
+        parameters=[{'use_sim_time': use_sim_time}]
+    )
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config_file],
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}]
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation time if true'
+        ),
+        robot_state_publisher,
+        joint_state_publisher_gui,
+        rviz,
+    ])

src/car_description/launch/car_gazebo.launch.py

@@ -0,0 +1,167 @@
+# car_gazebo.launch.py
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, Command
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+    LogInfo  # ✅ 确保导入
+)
+from launch.event_handlers import OnProcessStart, OnProcessExit  # ✅ 补上 OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    pkg_path = get_package_share_directory('car_description')
+    xacro_file = os.path.join(pkg_path, 'urdf', 'car.urdf.xacro')
+    config_path = os.path.join(pkg_path, 'config', 'controllers.yaml')
+
+    # 1. 解析 Xacro 文件
+    robot_description_content = Command(['xacro ', xacro_file, ' use_sim_time:=', use_sim_time])
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': ParameterValue(robot_description_content, value_type=str),
+            'use_sim_time': use_sim_time
+        }]
+    )
+
+    # 2. 启动 Gazebo
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+        ]),
+        launch_arguments={
+            'world': '',
+            'verbose': 'false',
+            'gui': 'true'
+        }.items()
+    )
+
+    # 3. 在 Gazebo 中生成机器人
+    spawn_entity = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        output='screen',
+        arguments=[
+            '-topic', 'robot_description',
+            '-entity', 'car_bot',
+            '-x', '0.0',
+            '-y', '0.0',
+            '-z', '0.0'
+        ],
+        parameters=[{'use_sim_time': use_sim_time}]
+    )
+
+    # ✅ 删除 controller_manager 节点！
+    # 它与 gazebo_ros2_control 冲突
+
+    # 4. 定义所有控制器 spawner（直接依赖 spawn_entity）
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        output='screen',
+    )
+
+    drive_group_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['drive_group_controller', '--controller-manager', '/controller_manager'],
+        output='screen',
+    )
+
+    steer_group_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['steer_group_controller', '--controller-manager', '/controller_manager'],
+        output='screen',
+    )
+
+    # ✅ 添加：发布零速度命令，防止漂移
+    send_zero_velocity = Node(
+        package='car_controller_pkg',  # 替换为你的包名
+        executable='send_zero_cmd',  # 确保你已创建该脚本
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}]
+    )
+
+    # 5. 控制启动顺序（全部依赖 spawn_entity）
+    delay_spawn_entity = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=robot_state_publisher,
+            on_start=[spawn_entity],
+        )
+    )
+
+    delay_joint_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=spawn_entity,
+            on_start=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    delay_drive_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=joint_state_broadcaster_spawner,
+            on_start=[drive_group_controller_spawner],
+        )
+    )
+
+    delay_steer_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=drive_group_controller_spawner,
+            on_start=[steer_group_controller_spawner],
+        )
+    )
+
+    # ✅ 发送零速度命令：在所有控制器 spawner 完成后
+    delay_send_zero = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=steer_group_controller_spawner,
+            on_exit=[
+                LogInfo(msg="✅ 所有控制器已加载，正在发送零速度命令以防止漂移..."),
+                send_zero_velocity,],
+        )
+    )
+
+    # 🚀 可选：启动自定义运动学节点
+    # dual_swerve_controller = Node(
+    #     package='dual_swerve_controller',
+    #     executable='dual_swerve_controller',
+    #     output='screen',
+    #     parameters=[{'use_sim_time': use_sim_time}]
+    # )
+    #
+    # delay_dual_swerve = RegisterEventHandler(
+    #     event_handler=OnProcessStart(
+    #         target_action=steer_group_controller_spawner,
+    #         on_start=[dual_swerve_controller],
+    #     )
+    # )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation time if true'
+        ),
+        robot_state_publisher,
+        gazebo,
+        delay_spawn_entity,
+        delay_joint_broadcaster_spawner,
+        delay_drive_controller_spawner,
+        delay_steer_controller_spawner,
+        delay_send_zero,  # ✅ 确保零命令发送
+        # delay_dual_swerve,
+    ])

src/car_description/launch/car_gazebo1.launch.py

@@ -0,0 +1,157 @@
+# car_gazebo.launch.py
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, Command
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessStart
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    pkg_path = get_package_share_directory('car_description')
+    xacro_file = os.path.join(pkg_path, 'urdf', 'car.urdf.xacro')
+    config_path = os.path.join(pkg_path, 'config', 'controllers.yaml')
+
+    # 1. 解析 Xacro 文件
+    robot_description_content = Command(['xacro ', xacro_file, ' use_sim_time:=', use_sim_time])
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': ParameterValue(robot_description_content, value_type=str),
+            'use_sim_time': use_sim_time
+        }]
+    )
+
+    # 2. 启动 Gazebo
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+        ]),
+        launch_arguments={
+            'world': '',
+            'verbose': 'false',
+            'gui': 'true'
+        }.items()
+    )
+
+    # 3. 在 Gazebo 中生成机器人
+    spawn_entity = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        output='screen',
+        arguments=[
+            '-topic', 'robot_description',
+            '-entity', 'car_bot',
+            '-x', '0.0',
+            '-y', '0.0',
+            '-z', '0.0'
+        ],
+        parameters=[{'use_sim_time': use_sim_time}]
+    )
+
+    # 4. 启动 controller_manager
+    controller_manager = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[{'robot_description': robot_description_content},
+                    config_path,
+                    {'use_sim_time': use_sim_time}],
+        output='screen',
+    )
+
+    # 5. 定义所有控制器 spawner
+    # 注意：名称必须与 controllers.yaml 中一致！
+
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+    )
+
+    drive_group_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['drive_group_controller', '--controller-manager', '/controller_manager'],
+    )
+
+    steer_group_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['steer_group_controller', '--controller-manager', '/controller_manager'],
+    )
+
+    # 6. 控制启动顺序
+    delay_spawn_entity = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=robot_state_publisher,
+            on_start=[spawn_entity],
+        )
+    )
+
+    delay_controller_manager = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=spawn_entity,
+            on_start=[controller_manager],
+        )
+    )
+
+    delay_joint_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=controller_manager,
+            on_start=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    delay_drive_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=joint_state_broadcaster_spawner,
+            on_start=[drive_group_controller_spawner],
+        )
+    )
+
+    delay_steer_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=drive_group_controller_spawner,
+            on_start=[steer_group_controller_spawner],
+        )
+    )
+
+    # 🚀 启动自定义运动学节点（可选）
+    # dual_swerve_controller = Node(
+    #     package='dual_swerve_controller',
+    #     executable='dual_swerve_controller',
+    #     output='screen',
+    #     parameters=[{'use_sim_time': use_sim_time}]
+    # )
+
+    # delay_dual_swerve = RegisterEventHandler(
+    #     event_handler=OnProcessStart(
+    #         target_action=steer_group_controller_spawner,
+    #         on_start=[dual_swerve_controller],
+    #     )
+    # )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation time if true'
+        ),
+        robot_state_publisher,
+        gazebo,
+        delay_spawn_entity,
+        delay_controller_manager,
+        delay_joint_broadcaster_spawner,
+        delay_drive_controller_spawner,
+        delay_steer_controller_spawner,
+        # delay_dual_swerve,  # 如果你写了自定义节点，取消注释
+    ])

src/car_description/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_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="lk@todo.todo">lk</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/car_description/rviz/urdf.rviz

@@ -0,0 +1,40 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+    - Alpha: 0.8
+      Class: rviz_default_plugins/RobotModel
+      Description Source: Topic
+      Description Topic:
+        Value: /robot_description
+      Enabled: true
+      Name: RobotModel
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Value: true
+  Global Options:
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 1.7
+      Name: Current View
+      Pitch: 0.33
+      Value: Orbit (rviz)
+      Yaw: 5.5
+Window Geometry:
+  Height: 800
+  Width: 1200

src/car_description/scripts/move.py

@@ -0,0 +1,146 @@
+#! /usr/bin/env python3
+
+import rclpy
+import math
+import numpy as np
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+
+def euler_from_quaternion(quaternion):
+    """
+    Converts quaternion (w in last place) to euler roll, pitch, yaw
+    quaternion = [x, y, z, w]
+    """
+    x = quaternion.x
+    y = quaternion.y
+    z = quaternion.z
+    w = quaternion.w
+
+    sinr_cosp = 2 * (w * x + y * z)
+    cosr_cosp = 1 - 2 * (x * x + y * y)
+    roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+    sinp = 2 * (w * y - z * x)
+    pitch = np.arcsin(sinp)
+
+    siny_cosp = 2 * (w * z + x * y)
+    cosy_cosp = 1 - 2 * (y * y + z * z)
+    yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+    return roll, pitch, yaw
+
+class CarController(Node):
+    def __init__(self):
+        super().__init__('car_controller')
+
+        # 目标位置
+        self.target_x = -2.0  # 目标 x 坐标
+        self.target_y = 2.0  # 目标 y 坐标
+        self.current_x = 0.0  # 当前 x 坐标
+        self.current_y = 0.0  # 当前 y 坐标
+        self.roll=0.0
+        self.pitch=0.0
+        self.yaw=0.0
+
+        # 订阅当前位置
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            '/odom',
+            self.odom_callback,
+            10
+        )
+
+        # 发布控制指令
+        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+        # 标记当前步骤
+        self.current_step = 1
+
+        # 角速度
+        self.angular_velocity = math.pi / 2  # 90 度/秒
+        self.timer=None
+        self.first_yaw =None
+
+    def odom_callback(self, odom_msg):
+        self.current_x = odom_msg.pose.pose.position.x
+        self.current_y = odom_msg.pose.pose.position.y
+        orientation=odom_msg.pose.pose.orientation
+        self.roll, self.pitch, self.yaw = euler_from_quaternion(orientation)
+        self.get_logger().info('Current Position: x={:.2f}, y={:.2f}, roll={:.2f}, pitch={:.2f}, yaw={:.2f}'.format(self.current_x, self.current_y, self.roll, self.pitch, self.yaw))
+
+
+        if self.current_step == 1:  # 走完 x 轴
+            if abs(self.current_x - self.target_x) < 0.05:
+                self.get_logger().info('X direction target position reached!')
+                self.current_step = 2  # 进入第二步：旋转
+                self.stop_car()
+                return
+
+            cmd_vel = Twist()
+            if self.current_x < self.target_x:
+                cmd_vel.linear.x = 0.5  # 正向线速度，可根据需要进行调整
+            else:
+                cmd_vel.linear.x = -0.5  # 反向线速度，可根据需要进行调整
+
+            self.cmd_vel_pub.publish(cmd_vel)
+
+        elif self.current_step == 2:  # 根据 y 轴坐标差选择旋转方向
+            dy = self.target_y - self.current_y
+            if dy > 0:
+                angle = math.pi / 2  # 向左旋转 90 度
+            else :
+                angle = -math.pi / 2  # 向右旋转 90 度
+            
+            if not self.first_yaw:
+                self.first_yaw=self.yaw+angle
+
+            if abs(self.first_yaw-self.yaw)>0.05:
+                cmd_vel = Twist()
+                cmd_vel.linear.x = 0.0
+                cmd_vel.angular.z = angle
+                self.cmd_vel_pub.publish(cmd_vel)   
+            else:
+                cmd_vel = Twist()
+                self.cmd_vel_pub.publish(cmd_vel)   
+                self.current_step = 3      
+            # if self.get_clock().now().nanoseconds/1e9-self.first_time<controllers.yaml.0:
+            #     cmd_vel = Twist()
+            #     cmd_vel.linear.x = 0.0
+            #     cmd_vel.angular.z = angle
+            #     self.cmd_vel_pub.publish(cmd_vel)
+            # else:
+            #     cmd_vel = Twist()
+            #     self.cmd_vel_pub.publish(cmd_vel)
+            #     self.current_step == 3
+
+        elif self.current_step == 3:  # 走完 y 轴
+            if abs(self.current_y - self.target_y) < 0.05:
+                self.get_logger().info('Target position reached!')
+                self.stop_car()
+                return
+
+            cmd_vel = Twist()
+            cmd_vel.linear.x = 0.5  # 前进线速度，可根据需要进行调整
+            self.cmd_vel_pub.publish(cmd_vel)
+
+    def stop_car(self):
+        cmd_vel = Twist()
+        self.cmd_vel_pub.publish(cmd_vel)
+
+
+
+    def move_to_target(self):
+        while rclpy.ok():
+            rclpy.spin_once(self)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    car_controller = CarController()
+    car_controller.move_to_target()
+
+
+if __name__ == '__main__':
+    main()

src/car_description/urdf/car.urdf.xacro

@@ -0,0 +1,308 @@
+<?xml version="1.0"?>
+<robot name="MID-FRAME9" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- =============== 材质定义 =============== -->
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="caster_wheel">
+    <color rgba="0.792 0.820 0.933 1.0"/>
+  </material>
+
+  <!-- =============== 主车体 base_link =============== -->
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0141670180033461 -0.00232520969743732 0.134535762309552" rpy="0 0 0"/>
+      <mass value="18.515886834371"/>
+      <inertia 
+        ixx="0.186692399010611" 
+        ixy="0.000823364078481365" 
+        ixz="0.000200143619649957"
+        iyy="0.172507063818075" 
+        iyz="1.92404238588556E-05" 
+        izz="0.333955223468975"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/base_link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- =============== 右前轮组件 =============== -->
+  <link name="Right_wheel_f_Link">
+    <inertial>
+      <origin xyz="0.0168008624736486 0.000455606969659295 -0.0638796908852731" rpy="0 0 0"/>
+      <mass value="0.673619090805562"/>
+      <inertia 
+        ixx="0.00176942727842739" 
+        ixy="-1.83816942573029E-09" 
+        ixz="-1.23694077314799E-05"
+        iyy="0.00178291468302619" 
+        iyz="6.09770090461609E-12" 
+        izz="0.00316410220887713"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_f_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_f_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Right_wheel_f_joint" type="revolute">
+    <origin xyz="0.330000028829699 -0.144999999455946 0.166500000000008" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="Right_wheel_f_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10.0" velocity="2.0"/>
+  </joint>
+
+  <link name="Right_wheel_g_Link">
+    <inertial>
+      <origin xyz="4.88498130835069E-15 -3.9013237085328E-13 -1.02690605380262E-07" rpy="0 0 0"/>
+      <mass value="0.754437061193079"/>
+      <inertia 
+        ixx="0.0012385312601574" 
+        ixy="-1.24352392257989E-17" 
+        ixz="-2.0199038489362E-16"
+        iyy="0.00221193431458472" 
+        iyz="2.07821845128484E-17" 
+        izz="0.00123864367215251"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_g_Link.STL"/>
+      </geometry>
+      <material name="caster_wheel"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_g_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Right_wheel_g_joint" type="continuous">
+    <origin xyz="-0.0215000000000098 0 -0.121506249511771" rpy="0 0 0"/>
+    <parent link="Right_wheel_f_Link"/>
+    <child link="Right_wheel_g_Link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <!-- =============== 左后轮组件 =============== -->
+  <link name="Left_wheel_back_Link">
+    <inertial>
+      <origin xyz="-0.0209669184954023 -0.000455606969659739 -0.0638796908852732" rpy="0 0 0"/>
+      <mass value="0.673619090805563"/>
+      <inertia 
+        ixx="0.0017694272784274" 
+        ixy="-1.83816942813998E-09" 
+        ixz="1.23694077314761E-05"
+        iyy="0.0017829146830262" 
+        iyz="-6.09770039528707E-12" 
+        izz="0.00316410220887713"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Left_wheel_back_joint" type="revolute">
+    <origin xyz="-0.330000028829851 0.145000000543845 0.166500000000008" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="Left_wheel_back_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10.0" velocity="2.0"/>
+  </joint>
+
+  <link name="Left_wheel_back_g_Link">
+    <inertial>
+      <origin xyz="-4.94049245958195E-15 3.9013237085328E-13 -1.02690605546796E-07" rpy="0 0 0"/>
+      <mass value="0.754437061193078"/>
+      <inertia 
+        ixx="0.0012385312601574" 
+        ixy="-1.25551092559053E-17" 
+        ixz="2.02197944367259E-16"
+        iyy="0.00221193431458472" 
+        iyz="-1.31772991193631E-17" 
+        izz="0.00123864367215251"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_g_Link.STL"/>
+      </geometry>
+      <material name="caster_wheel"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_g_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Left_wheel_back_g_joint" type="continuous">
+    <origin xyz="0.0135000000000094 0 -0.121506249511771" rpy="0 0 0"/>
+    <parent link="Left_wheel_back_Link"/>
+    <child link="Left_wheel_back_g_Link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <!-- =============== 万向轮 1（左后） =============== -->
+  <link name="wanxiang1_Link">
+    <inertial>
+      <origin xyz="0 0 -0.052084" rpy="0 0 0"/>
+      <mass value="1.9859"/>
+      <inertia 
+        ixx="0.004691" 
+        ixy="-3.5657E-36" 
+        ixz="1.9066E-19"
+        iyy="0.004691" 
+        iyz="8.2159E-21" 
+        izz="0.0057062"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang1_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="wanxiang1_joint" type="fixed">
+    <!-- 修复：z 从 0.094541 调整为 0.035 -->
+    <origin xyz="-0.33 -0.145 0.09454" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="wanxiang1_Link"/>
+    <axis xyz="0 0 -1"/>
+  </joint>
+
+  <!-- =============== 万向轮 2（右前） =============== -->
+  <link name="wanxiang2_Link">
+    <inertial>
+      <origin xyz="5.5511E-17 0 -0.052084" rpy="0 0 0"/>
+      <mass value="1.9859"/>
+      <inertia 
+        ixx="0.004691" 
+        ixy="-3.5657E-36" 
+        ixz="7.8139E-20"
+        iyy="0.004691" 
+        iyz="8.2159E-21" 
+        izz="0.0057062"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang2_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="wanxiang2_joint" type="fixed">
+    <!-- 修复：z 从 0.09454 调整为 0.035 -->
+    <origin xyz="0.33 0.145 0.09454" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="wanxiang2_Link"/>
+    <axis xyz="0 0 -1"/>
+  </joint>
+
+  <!-- =============== ros2_control Plugin =============== -->
+  <ros2_control name="DualSwerveControl" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+
+    <!-- 右前舵轮：转向 + 驱动 -->
+    <joint name="Right_wheel_f_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="Right_wheel_g_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="position"/>
+    </joint>
+
+    <!-- 左后舵轮：转向 + 驱动 -->
+    <joint name="Left_wheel_back_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="Left_wheel_back_g_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="position"/>
+    </joint>
+  </ros2_control>
+
+  <!-- =============== Gazebo Plugin =============== -->
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+      <robotSimType>gazebo_ros2_control/GazeboSystem</robotSimType>
+      <parameters>$(find car_description)/config/controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+  <!-- =============== 降低万向轮摩擦 =============== -->
+  <gazebo reference="wanxiang1_Link">
+    <mu1 value="0.1"/>
+    <mu2 value="0.1"/>
+    <kp>1000000.0</kp>
+    <kd>1.0</kd>
+    <damping>0.1</damping>
+  </gazebo>
+
+  <gazebo reference="wanxiang2_Link">
+    <mu1 value="0.1"/>
+    <mu2 value="0.1"/>
+    <kp>1000000.0</kp>
+    <kd>1.0</kd>
+    <damping>0.1</damping>
+  </gazebo>
+
+</robot>

src/car_description/urdf/car.urdf（复件）.xacro

@@ -0,0 +1,287 @@
+<?xml version="1.0"?>
+<robot name="MID-FRAME9" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- =============== 材质定义 =============== -->
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="caster_wheel">
+    <color rgba="0.792 0.820 0.933 1.0"/>
+  </material>
+
+  <!-- =============== 主车体 base_link =============== -->
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0141670180033461 -0.00232520969743732 0.134535762309552" rpy="0 0 0"/>
+      <mass value="18.515886834371"/>
+      <inertia 
+        ixx="0.186692399010611" 
+        ixy="0.000823364078481365" 
+        ixz="0.000200143619649957"
+        iyy="0.172507063818075" 
+        iyz="1.92404238588556E-05" 
+        izz="0.333955223468975"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/base_link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- =============== 右前轮组件 =============== -->
+  <link name="Right_wheel_f_Link">
+    <inertial>
+      <origin xyz="0.0168008624736486 0.000455606969659295 -0.0638796908852731" rpy="0 0 0"/>
+      <mass value="0.673619090805562"/>
+      <inertia 
+        ixx="0.00176942727842739" 
+        ixy="-1.83816942573029E-09" 
+        ixz="-1.23694077314799E-05"
+        iyy="0.00178291468302619" 
+        iyz="6.09770090461609E-12" 
+        izz="0.00316410220887713"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_f_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_f_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Right_wheel_f_joint" type="revolute">
+    <origin xyz="0.330000028829699 -0.144999999455946 0.166500000000008" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="Right_wheel_f_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-1.57" upper="1.57" effort="10.0" velocity="2.0"/>
+  </joint>
+
+  <link name="Right_wheel_g_Link">
+    <inertial>
+      <origin xyz="4.88498130835069E-15 -3.9013237085328E-13 -1.02690605380262E-07" rpy="0 0 0"/>
+      <mass value="0.754437061193079"/>
+      <inertia 
+        ixx="0.0012385312601574" 
+        ixy="-1.24352392257989E-17" 
+        ixz="-2.0199038489362E-16"
+        iyy="0.00221193431458472" 
+        iyz="2.07821845128484E-17" 
+        izz="0.00123864367215251"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_g_Link.STL"/>
+      </geometry>
+      <material name="caster_wheel"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Right_wheel_g_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Right_wheel_g_joint" type="continuous">
+    <origin xyz="-0.0215000000000098 0 -0.121506249511771" rpy="0 0 0"/>
+    <parent link="Right_wheel_f_Link"/>
+    <child link="Right_wheel_g_Link"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <!-- =============== 左后轮组件 =============== -->
+  <link name="Left_wheel_back_Link">
+    <inertial>
+      <origin xyz="-0.0209669184954023 -0.000455606969659739 -0.0638796908852732" rpy="0 0 0"/>
+      <mass value="0.673619090805563"/>
+      <inertia 
+        ixx="0.0017694272784274" 
+        ixy="-1.83816942813998E-09" 
+        ixz="1.23694077314761E-05"
+        iyy="0.0017829146830262" 
+        iyz="-6.09770039528707E-12" 
+        izz="0.00316410220887713"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Left_wheel_back_joint" type="revolute">
+    <origin xyz="-0.330000028829851 0.145000000543845 0.166500000000008" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="Left_wheel_back_Link"/>
+    <axis xyz="0 0 -1"/>
+    <limit lower="-1.57" upper="1.57" effort="10.0" velocity="2.0"/>
+  </joint>
+
+  <link name="Left_wheel_back_g_Link">
+    <inertial>
+      <origin xyz="-4.94049245958195E-15 3.9013237085328E-13 -1.02690605546796E-07" rpy="0 0 0"/>
+      <mass value="0.754437061193078"/>
+      <inertia 
+        ixx="0.0012385312601574" 
+        ixy="-1.25551092559053E-17" 
+        ixz="2.02197944367259E-16"
+        iyy="0.00221193431458472" 
+        iyz="-1.31772991193631E-17" 
+        izz="0.00123864367215251"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_g_Link.STL"/>
+      </geometry>
+      <material name="caster_wheel"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/Left_wheel_back_g_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="Left_wheel_back_g_joint" type="continuous">
+    <origin xyz="0.0135000000000094 0 -0.121506249511771" rpy="0 0 0"/>
+    <parent link="Left_wheel_back_Link"/>
+    <child link="Left_wheel_back_g_Link"/>
+    <axis xyz="0 -1 0"/>
+  </joint>
+
+  <!-- =============== 万向轮 1（左后） =============== -->
+  <link name="wanxiang1_Link">
+    <inertial>
+      <origin xyz="0 0 -0.052084" rpy="0 0 0"/>
+      <mass value="1.9859"/>
+      <inertia 
+        ixx="0.004691" 
+        ixy="-3.5657E-36" 
+        ixz="1.9066E-19"
+        iyy="0.004691" 
+        iyz="8.2159E-21" 
+        izz="0.0057062"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang1_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="wanxiang1_joint" type="fixed">
+    <origin xyz="-0.33 -0.145 0.094541" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="wanxiang1_Link"/>
+    <axis xyz="0 0 -1"/>
+  </joint>
+
+  <!-- =============== 万向轮 2（右前） =============== -->
+  <link name="wanxiang2_Link">
+    <inertial>
+      <origin xyz="5.5511E-17 0 -0.052084" rpy="0 0 0"/>
+      <mass value="1.9859"/>
+      <inertia 
+        ixx="0.004691" 
+        ixy="-3.5657E-36" 
+        ixz="7.8139E-20"
+        iyy="0.004691" 
+        iyz="8.2159E-21" 
+        izz="0.0057062"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang2_Link.STL"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="file://$(find car_description)/meshes/wanxiang2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="wanxiang2_joint" type="fixed">
+    <origin xyz="0.33 0.145 0.09454" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="wanxiang2_Link"/>
+    <axis xyz="0 0 -1"/>
+  </joint>
+
+  <!-- =============== ros2_control Plugin =============== -->
+  <ros2_control name="DualSwerveControl" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+
+  <!-- 右前舵轮：转向 + 驱动 -->
+    <joint name="Right_wheel_f_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="Right_wheel_g_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="position"/>
+    </joint>
+
+  <!-- 左后舵轮：转向 + 驱动 -->
+    <joint name="Left_wheel_back_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="Left_wheel_back_g_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="velocity"/>
+      <state_interface name="position"/>
+    </joint>
+  </ros2_control>
+<!-- =============== Gazebo Plugin =============== -->
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+      <robotSimType>gazebo_ros2_control/GazeboSystem</robotSimType>
+      <parameters>$(find car_description)/config/controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>