Gazebo仿真时小车在自动旋转

[kalami_fishros]

我做了一个简易的两轮差速小车模型，启动仿真后没有发出任何控制指令，两个驱动轮莫名旋转。

环境：

Ubuntu 22.04 + ROS2 Humble + Gazebo Classic

尝试了这些方式，问题依然存在：

• 起初以为是惯性张量问题，在solidwork建模(模型没有用到URDF中)并设置材料密度后，把软件计算的质量和惯性张量复制到URDF文件中，无法解决问题
• 为left_wheel_joint和right_wheel_joint都添加了dynamics，无法解决问题
• 增加摩擦系数到2.0，无法解决问题
• 调整轮子与底面接触距离，无法解决问题

注：在Gazebo中将重力调为0后模型飘在空中，但是点击JOINTS查看left_wheel_joint和right_wheel_joint时有个属性angle_0在不断变化

仿真图片

URDF文件内容：

<?xml version="1.0"?>
<robot name="test_robot">
    <link name="base_footprint" >
    </link>

    <link name="base_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.4 0.2 0.1"/>
            </geometry>
            <material name="red">
                <color rgba="1.0 0.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.4 0.2 0.1"/>
            </geometry>
            <material name="red">
                <color rgba="1.0 0.0 0.0 1.0"/>
            </material>
        </collision>
        <inertial>
            <mass value="4.0"/>
            <inertia ixx="0.016667" ixy="0" ixz="0" iyy="0.056667" iyz="0" izz="0.066667"/>
        </inertial>
    </link>
    <joint name="base_joint" type="fixed">
        <origin xyz="0.0 0.0 0.075" rpy="0.0 0.0 0.0"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="1.570796327 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.025" length="0.01"/>
            </geometry>
            <density>1000.0</density>
            <material name="green">
                <color rgba="0.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="1.570796327 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.025" length="0.01"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="0.019635"/>
            <inertia ixx="3.232e-6" iyy="6.136e-6" izz="3.232e-6" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <joint name="left_wheel_joint" type="continuous">
        <origin xyz="-0.15 0.105 -0.051" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="left_wheel_link"/>
        <axis xyz="0.0 1.0 0.0" />
        <dynamics damping="1.0" friction="0.2"/>
        <limit lower="0.0" upper="0.0" effort="10.0" velocity="0.0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="1.570796327 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.025" length="0.01"/>
            </geometry>
            <density>1000.0</density>
            <material name="blue">
                <color rgba="0.0 0.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="1.570796327 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.025" length="0.01"/>
            </geometry>
            <density>1000.0</density>
        </collision>
        <inertial>
            <mass value="0.019635"/>
            <inertia ixx="3.232e-6" iyy="6.136e-6" izz="3.232e-6" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="-0.15 -0.105 -0.051" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="right_wheel_link"/>
        <axis xyz="0.0 1.0 0.0" />
        <dynamics damping="1.0" friction="0.2"/>
        <limit lower="0.0" upper="0.0" effort="10.0" velocity="0.0"/>
    </joint>

    <link name="front_wheel_link">
        <visual >
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <sphere radius="0.025"/>
            </geometry>
            <density>1000.0</density>
            <material name="yellow">
                <color rgba="1.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <sphere radius="0.025"/>
            </geometry>
            <density>1000.0</density>
        </collision>
        <inertial>
            <mass value="0.06545"/>
            <inertia ixx="1.636e-5" iyy="1.636e-5" izz="1.636e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <joint name="front_wheel_joint" type="fixed">
        <origin xyz="0.15 0.0 -0.051" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="front_wheel_link"/>
        <axis xyz="0.0 0.0 0.0"/>
        <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0"/>
    </joint>


    <!--Gazebo材质与颜色-->
    <gazebo reference="base_link">
        <material>Gazebo/Red</material>
    </gazebo>
    <gazebo reference="left_wheel_link">
        <material>Gazebo/Green</material>
    </gazebo>
    <gazebo reference="right_wheel_link">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="front_wheel_link">
        <material>Gazebo/Yellow</material>
    </gazebo>

    <!--Gazebo摩擦系数-->
    <gazebo reference="left_wheel_link">
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="right_wheel_link">
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>

    <!--ROS2控制框架(ros2_control)-->
    <ros2_control name="GazeboSim" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="left_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10.0</param>
                <param name="max">10.0</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
        <joint name="right_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10.0</param>
                <param name="max">10.0</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>

    <!--Gazebo仿真插件:加载控制框架-->
    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>/home/yahboom/wheeltec_ros2/install/wheeltec_robot_urdf/share/wheeltec_robot_urdf/configs/diff_drive_controller.yaml</parameters>
            <robot_param>robot_description</robot_param>
        </plugin>
    </gazebo>

</robot>

差速控制器配置文件：

## configs/diff_drive.yaml
/controller_manager:
  ros__parameters:
    update_rate: 100
    # 声明使用的控制器
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

# 关节状态广播器配置
joint_state_broadcaster:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint

# 差速控制器核心配置
diff_drive_controller:
  ros__parameters:
    use_stamped_vel: false
    cmd_vel_topic: "/cmd_vel_unstamped"
    odom_topic: "/odom"
    # 输入/输出话题
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    
    wheel_separation: 0.21          # 左右轮间距，单位米
    wheel_radius: 0.025             # 轮子半径，单位米
    
    # 控制与里程计参数
    publish_rate: 50.0              # 里程计发布频率
    odom_frame_id: odom
    base_frame_id: base_footprint
    
    # 开放控制回路
    open_loop: false
    enable_odom_tf: true
    
    # 命令接口
    cmd_vel_timeout: 0.5
    # 速度指令限制
    linear:
      x:
        has_velocity_limits: true
        max_velocity: 0.5
        has_acceleration_limits: true
        max_acceleration: 1.0
    angular:
      z:
        has_velocity_limits: true
        max_velocity: 1.0
        has_acceleration_limits: true
        max_acceleration: 2.0

启动文件：

import os
import launch
import launch.launch_description_sources
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python import get_package_share_path
from launch.actions import ExecuteProcess,RegisterEventHandler
from launch.event_handlers import OnProcessExit

def generate_launch_description():

    pkg_share = get_package_share_directory('wheeltec_robot_urdf')
    urdf_file_path = os.path.join(pkg_share,'test','test_diff_robot.urdf')
    with open(urdf_file_path,'r') as f:
        urdf_description_context = f.read()

    gazebo_pkg_share = get_package_share_path('gazebo_ros')
    gazebo_world_path = os.path.join(pkg_share,'world','room2.world')

    # 机器人状态发布器
    action_robot_state_pubilsher = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        name = 'robot_state_publisher',
        parameters=[{'robot_description':urdf_description_context}]
    )

    # 启动Gazebo仿真器(包含gzserver和gzclient)
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            os.path.join(gazebo_pkg_share,'launch','gazebo.launch.py')
        ),
        launch_arguments={
            'world':gazebo_world_path,
            'verbose':'true',
            'use_sim_time': 'true'
        }.items()
    )

    # 将机器人模型生成到Gazebo世界中
    action_spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', 'robot_description', '-entity', 'test_robot'],
        output='screen'
    )

    # 在spawn_entity启动后再启动joint_state_broadcaster和diff_drive_controller
    action_load_joint_state_controller = ExecuteProcess(
        cmd = 'ros2 control load_controller joint_state_broadcaster --set-state active'.split(' '),
        output = 'screen'
    )

    action_load_diff_drive_controller = ExecuteProcess(
        cmd = 'ros2 control load_controller diff_drive_controller --set-state active'.split(' '),
        output = 'screen'
    )

    delay_controllers = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=action_spawn_entity,
            on_exit=[action_load_joint_state_controller, action_load_diff_drive_controller]
        )
    )

    return LaunchDescription([
        action_robot_state_pubilsher,
        action_launch_gazebo,
        action_spawn_entity,
        delay_controllers,
    ])