ROS2与gazebo联合仿真自定义机械臂（ros2_control）

1. 准备实际的三维模型文件（stl二进制格式的）

2. 编写机械臂的urdf文件，也可以改装为xacro文件\

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="r6bot">
    <xacro:property name="ModelPathHead"
        value="file://"到机械臂三维模型的绝对路径" />
    <xacro:property name="pi" value="3.1415926" />
    <link name="base_link">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_0.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_0.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_1">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_1.stl" />
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_1.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_2">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_2.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyx="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_2.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_3">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_3.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyx="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_3.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_4">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_4.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyx="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_4.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_5">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_5.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyx="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_5.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <link name="link_6">
        <visual>
            <geometry>
                <mesh filename="${ModelPathHead}link_6.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="Red">
                <color rgba="1 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyx="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="${ModelPathHead}link_6.stl" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <!-- <link name="world"/> -->

    <joint name="base_joint" type="fixed">
        <parent link="world" />
        <child link="base_link" />
        <origin xyz="0 0 0" rpy="0 0 0" />
        <axis xyz="0 0 1" />
    </joint>

  <!-- joints - main serial chain -->
    <joint name="Joint_1" type="revolute">
        <parent link="base_link" />
        <child link="link_1" />
        <origin xyz="0 0 0.061584" rpy="0 0 0" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>

    <joint name="Joint_2" type="revolute">
        <parent link="link_1" />
        <child link="link_2" />
        <origin xyz="-0.101717 0 0.182284" rpy="${-pi/2} -${pi/3} ${pi/2}" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>

    <joint name="Joint_3" type="revolute">
        <parent link="link_2" />
        <child link="link_3" />
        <origin xyz="0.685682 0 0.041861" rpy="0 ${pi} ${pi+pi/2}" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>

    <joint name="Joint_4" type="revolute">
        <parent link="link_3" />
        <child link="link_4" />
        <origin xyz="0.518777 0 0.067458" rpy="0 ${pi} ${pi+pi/6}" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>

    <joint name="Joint_5" type="revolute">
        <parent link="link_4" />
        <child link="link_5" />
        <origin xyz="0.112654 0 0.110903" rpy="${pi/2} ${pi} ${pi/2}" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>

    <joint name="Joint_6" type="revolute">
        <parent link="link_5" />
        <child link="link_6" />
        <origin xyz="-0.085976 0 0.133436" rpy="0 ${-pi/2} 0" />
        <axis xyz="0 0 1" />
        <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5" />
    </joint>


    <gazebo reference="base_link">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_1">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_2">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_3">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_4">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_5">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

    <gazebo reference="link_6">
        <material>Gazebo/Purple</material>
        <self_collide>false</self_collide>
        <gravity>false</gravity>
    </gazebo>

</robot>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="Staubli">
    <xacro:property name="UrdfPathHead" value="urdf文件路径" />
    <!-- <xacro:include filename="${UrdfPathHead}robot.urdf.xacro" /> -->
    <link name="world" />
    <xacro:include filename="${UrdfPathHead}r6bot.urdf.xacro" />

    <xacro:include filename="${UrdfPathHead}robot_ros_control.urdf.xacro" />
</robot>

3.编写控制文件(xacro)

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="Staubli" >
    <ros2_control name="GazeboSystem" type="system">
        <hardware>
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        
        <joint name="Joint_1">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="Joint_2">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="Joint_3">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="Joint_4">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="Joint_5">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="Joint_6">
            <command_interface name="position" />
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>

    <gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <!-- 这个是编写的控制器文件的路径 -->
          <parameters>$(find gazebo_robot)/config/gazebo_robot_control.yaml</parameters>
      </plugin>
  </gazebo>

</robot>

4. 定义控制器

controller_manager:
  ros__parameters:
    update_rate: 100
    sue_sim_time: true

    arm_controller:
      type: position_controllers/JointGroupPositionController // ros2_control 里面定义的控制器

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster   //固定的

arm_controller:
  ros__parameters:
    joints:
      - Joint_1  // 注意和机械臂的urdf中定义的关节角保持一致
      - Joint_2
      - Joint_3
      - Joint_4
      - Joint_5
      - Joint_6
    command_interface:
      - position

    state_interface:
      - position
      - velocity

5.编写launch文件

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess,RegisterEventHandler
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch.substitutions import Command
from launch.event_handlers import OnProcessExit
import launch.actions
import xacro



def generate_launch_description():
    robot_name_in_model ="r6bot"
    package_name='gazebo_robot'
    urdf_name = "r6bot_main.urdf.xacro"
    ld = LaunchDescription()
    #找到自己的ros包
    pkg_share = FindPackageShare(package='gazebo_robot').find('gazebo_robot')
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    start_gazebo_cmd =  ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen')
    
    doc = xacro.parse(open(urdf_model_path))
    xacro.process_doc(doc)
    params = {'robot_description': doc.toxml()}
    

    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )

    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-topic', 'robot_description','-entity', 'robot_gazebo'], output='screen')
    

#将自己定义的环境模型，写成类似与gazebo自带的模型文件形式，注意有几个文件定义。
#自己如何撰写gazebo模型，可以参考网上的教程 最后需要export路径，这里用python实现
    model_path = 自定义的模型文件的路径"

    if "GAZEBO_MODEL_PATH" in os.environ:
        os.environ["GAZEBO_MODEL_PATH"] += ":" + model_path
    else:
        os.environ["GAZEBO_MODEL_PATH"] = model_path
    EnvModel = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
#'.sdf文件路径'是自定义模型中有个sdf文件的，直接传入这个文件的路径。
        arguments=['-entity', 'demo', '-file', '.sdf文件路径'], output='screen')
    
    
    joint_broad_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"]
    )

    arm_broad_spawner=Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_controller"]
    )

    ld.add_action(start_gazebo_cmd)
    ld.add_action(node_robot_state_publisher)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(EnvModel)
    ld.add_action(joint_broad_spawner)
    ld.add_action(arm_broad_spawner)

    return ld

后续把这部分实际代码分享到git中。路径：Ros2_Control: Ros2与gazebo联合仿真