ROS2+MoveIt2+Gazebo中的仿真（快速通关版）

参考 韭菜钟 和 鱼香ROS 的两篇文章（侵权删）：

1. 在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手-CSDN博客

2. 动手学Moveit2|使用配置助手创建自己机械臂的功能包_yuxiangros 动手学moveit2-CSDN博客

一、软件安装

有ROS2、Moveit2、moveit-setup-assistant、Gazebo，这里就不细节说明了，可看上面参考文章

二、依赖安装

上面都有，我这里集齐一下（可能也不全，缺的评论一下）：

sudo apt install ros-humble-moveit-*

sudo apt install ros-humble-controller-manager -y

sudo apt install ros-humble-joint-trajectory-controller ros-humble-joint-state-broadcaster -y

三、创建工作空间及功能包

1. 打开终端（ctrl+alt+t），直接复制下面语句并运行

mkdir -p myRobot/src
cd myRobot/src
ros2 pkg create mybot_description --build-type ament_python
cd mybot_description
mkdir -p urdf
cd urdf
gedit six_arm.urdf

2. 将下面的机械臂urdf复制进打开的six_arm.urdf文档，保存后在终端Ctrl+C一下，关掉gedit文档编辑（此时把gazebo标签和ros2_control标签的注释掉了）:

<?xml version="1.0"?>
<robot name="six_arm">

    <!-- Base link -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="blue">
                <color rgba="0 0 1.0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Link 1 -->
    <link name="link1">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 1: rotation around X-axis -->
    <joint name="joint1" type="continuous">
        <parent link="base_link"/>
        <child link="link1"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
    </joint>

    <!-- Link 2 -->
    <link name="link2">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="red">
                <color rgba="0.8 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 2: rotation around Y-axis -->
    <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 3 -->
    <link name="link3">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="yellow">
                <color rgba="0.8 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 3: rotation around x-axis -->
    <joint name="joint3" type="continuous">
        <parent link="link2"/>
        <child link="link3"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 4 -->
    <link name="link4">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 4: rotation around X-axis -->
    <joint name="joint4" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
        <axis xyz="0 1 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 5 -->
    <link name="link5">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="purple">
                <color rgba="0.8 0 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 5: rotation around Y-axis -->
    <joint name="joint5" type="continuous">
        <parent link="link4"/>
        <child link="link5"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 6 -->
    <link name="link6">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
            <material name="pink">
                <color rgba="0.8 0.4 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 6: rotation around Z-axis -->
    <joint name="joint6" type="continuous">
        <parent link="link5"/>
        <child link="link6"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1"/>
    </joint>


    <!-- Used for fixing robot to Gazebo 'base_link' 将机械手的基座固定在世界坐标上-->
    <link name="world"/>

    <joint name="fixed" type="fixed">
        <parent link="world"/>
        <child link="base_link"/>
    </joint>

    <!-- <gazebo reference="base_link">
        <material>Gazebo/Black</material>
        <gravity>true</gravity>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link1">
        <material>Gazebo/Gray</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link2">
        <material>Gazebo/Red</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link3">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link4">
        <material>Gazebo/Green</material>
    </gazebo>
    <gazebo reference="link5">
        <material>Gazebo/Yellow</material>
    </gazebo>
    <gazebo reference="link6">
        <material>Gazebo/Orange</material>
    </gazebo> -->

    <!-- 在运行demo.launch.py时，需要注释这个ros2_control节点，因为它使用了xxx.ros2_control.xacro来生成了ros2_control节点-->
    <!-- <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="joint1">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint2">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint3">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint4">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint5">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint6">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control> -->

    <!-- <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find mybot)/config/ros2_controllers.yaml</parameters>
            <robot_param_node>robot_state_publisher</robot_param_node>
        </plugin>
    </gazebo> -->

</robot>

3. 检查文件数量是否一致，应该是9个文件。

cd ~/myRobot/src
tree

4. 开始使用moveit助手

cd ~/myRobot
colcon build
source install/setup.bash 
ros2 run moveit_setup_assistant moveit_setup_assistant

4.1 创建新配置包，点击Browse 找到 six_arm.urdf 路径，并点击 Load Files

4.2 设置自我碰撞，选择 Self Collisions，接着点击右边的 Generate Collision Matrix

4.3 定义规划组，选择 Planning Groups，接着依次如图操作

4.4 点击Robot Poses，并点击Add Pose（为第16步），并添加home及pos1，pos可以多添加几个，这里我至添加了一个作为演示

4.5 配置ros_control URDF Modiificatoins

4.6  配置ROS 2 Controllers

4.7 配置 Moveit Controllers

4.8  配置作者信息

4.9 最后配置文件的导出（可以提前在src目录下新建一个名为 mybot 的文件夹，用于存放导出的文件包，也可以照着下面一步一步进行）

5. 编译运行

5.1 在运行之前需要改这个文件 src/mybot_description/setup.py 中的三处地方，只需要把下面代码中的 "#这里"字样的三行代码复制粘贴到对应位置即可  （不能无脑整个文件替换，原因对照自查）

from setuptools import setup

from glob import glob #这里
import os #这里

package_name = 'mybot_description'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/**')), #这里
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='yong',
    maintainer_email='yong@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

5.2 编译运行 demo.launch.py

colcon build
ros2 launch mybot demo.launch.py

5.3 有个疑问：但是不知道为啥，这个只能Plan，不能Execute（不过在后续和gazebo一起开的时候可以），有大佬知道原因，可以在评论区解答一下，谢谢

6. Moveit2 和 Gazebo 关联

6.1 在 src/mybot/launch 路径下（和上面跑的 demo.launch.py 同路径），新建一个文件： gazebo.launch.py，并将下面代码直接复制进去保存

import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from launch.event_handlers import OnProcessExit

import xacro

import re
def remove_comments(text):
    pattern = r'<!--(.*?)-->'
    return re.sub(pattern, '', text, flags=re.DOTALL)

def generate_launch_description():
    robot_name_in_model = 'six_arm'
    package_name = 'mybot_description'
    urdf_name = "six_arm.urdf"

    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    # Start Gazebo server
    start_gazebo_cmd =  ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen')


    # 因为 urdf文件中有一句 $(find mybot) 需要用xacro进行编译一下才行
    xacro_file = urdf_model_path
    doc = xacro.parse(open(xacro_file))
    xacro.process_doc(doc)
    # params = {'robot_description': doc.toxml()}
    params = {'robot_description': remove_comments(doc.toxml())}

    # 启动了robot_state_publisher节点后，该节点会发布 robot_description 话题，话题内容是模型文件urdf的内容
    # 并且会订阅 /joint_states 话题，获取关节的数据，然后发布tf和tf_static话题.
    # 这些节点、话题的名称可不可以自定义？
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'use_sim_time': True}, params, {"publish_frequency":15.0}],
        output='screen'
    )

    # Launch the robot, 通过robot_description话题进行模型内容获取从而在gazebo中生成模型
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-topic', 'robot_description'], output='screen')


    # # Launch the robot, 这个是通过传递文件路径来在gazebo里生成模型.此时要求urdf文件里面没有xacro的语句
    # spawn_entity_cmd = Node(
    #     package='gazebo_ros', 
    #     executable='spawn_entity.py',
    #     arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')
    
    # node_robot_state_publisher = Node(
    #     package='robot_state_publisher',
    #     executable='robot_state_publisher',
    #     arguments=[urdf_model_path],
    #     parameters=[{'use_sim_time': True}],
    #     output='screen'
    # )

    # gazebo在加载urdf时，根据urdf的设定，会启动一个joint_states节点?
    # 关节状态发布器
    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_broadcaster'],
        output='screen'
    )

    # 路径执行控制器，也就是那个action？
    # 系统是如何知道有my_group_controller这个控制器的存在？
    load_joint_trajectory_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'my_group_controller'],
        output='screen'
    )

    # 用下面这两个估计是想控制好各个节点的启动顺序
    # 监听 spawn_entity_cmd，当其退出（完全启动）时，启动load_joint_state_controller？
    close_evt1 =  RegisterEventHandler( 
            event_handler=OnProcessExit(
                target_action=spawn_entity_cmd,
                on_exit=[load_joint_state_controller],
            )
    )
    # 监听 load_joint_state_controller，当其退出（完全启动）时，启动load_joint_trajectory_controller？
    # moveit是怎么和gazebo这里提供的action连接起来的？？
    close_evt2 = RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_joint_trajectory_controller],
            )
    )
    
    ld = LaunchDescription()

    ld.add_action(close_evt1)
    ld.add_action(close_evt2)

    ld.add_action(start_gazebo_cmd)
    ld.add_action(node_robot_state_publisher)
    ld.add_action(spawn_entity_cmd)

    return ld

6.2 将 src/mybot_description/urdf/six_arm.urdf 的内容中对于 ros2_control 和 gazebo 标签的注释打开，想偷懒的可以直接复制下面代码直接替换就好

<?xml version="1.0"?>
<robot name="six_arm">

    <!-- Base link -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="blue">
                <color rgba="0 0 1.0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Link 1 -->
    <link name="link1">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 1: rotation around X-axis -->
    <joint name="joint1" type="continuous">
        <parent link="base_link"/>
        <child link="link1"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
    </joint>

    <!-- Link 2 -->
    <link name="link2">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="red">
                <color rgba="0.8 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 2: rotation around Y-axis -->
    <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 3 -->
    <link name="link3">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="yellow">
                <color rgba="0.8 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 3: rotation around x-axis -->
    <joint name="joint3" type="continuous">
        <parent link="link2"/>
        <child link="link3"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 4 -->
    <link name="link4">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 4: rotation around X-axis -->
    <joint name="joint4" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
        <axis xyz="0 1 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 5 -->
    <link name="link5">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="purple">
                <color rgba="0.8 0 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 5: rotation around Y-axis -->
    <joint name="joint5" type="continuous">
        <parent link="link4"/>
        <child link="link5"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 6 -->
    <link name="link6">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
            <material name="pink">
                <color rgba="0.8 0.4 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 6: rotation around Z-axis -->
    <joint name="joint6" type="continuous">
        <parent link="link5"/>
        <child link="link6"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1"/>
    </joint>


    <!-- Used for fixing robot to Gazebo 'base_link' 将机械手的基座固定在世界坐标上-->
    <link name="world"/>

    <joint name="fixed" type="fixed">
        <parent link="world"/>
        <child link="base_link"/>
    </joint>

    <gazebo reference="base_link">
        <material>Gazebo/Black</material>
        <gravity>true</gravity>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link1">
        <material>Gazebo/Gray</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link2">
        <material>Gazebo/Red</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link3">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link4">
        <material>Gazebo/Green</material>
    </gazebo>
    <gazebo reference="link5">
        <material>Gazebo/Yellow</material>
    </gazebo>
    <gazebo reference="link6">
        <material>Gazebo/Orange</material>
    </gazebo>

    <!-- 在运行demo.launch.py时，需要注释这个ros2_control节点，因为它使用了xxx.ros2_control.xacro来生成了ros2_control节点-->
    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="joint1">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint2">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint3">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint4">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint5">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint6">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>

    <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find mybot)/config/ros2_controllers.yaml</parameters>
            <robot_param_node>robot_state_publisher</robot_param_node>
        </plugin>
    </gazebo>

</robot>

6.3 编译运行

colcon build
ros2 launch mybot gazebo.launch.py 

6.4 测试

6.4.1 先看看有没有action

ros2 action list

6.4.2 写个脚本

gedit my_send_goal.sh

打开后复制下面语句进去并保存后关闭

ros2 action send_goal /my_group_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{
  trajectory: {
    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6],
    points: [
      { positions: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], time_from_start: { sec: 0, nanosec: 500 } },
      { positions: [0.2, 0.5, 0.2 ,0.2, 0.2, 0.2], time_from_start: { sec: 5, nanosec: 500 } },
      { positions: [0.3, 0.3, 0.7, -0.5, 0.3, 0.3], time_from_start: { sec: 7, nanosec: 500 } },
      { positions: [0.4, 0.4, 0.9, 0.4, 0.4, 0.4], time_from_start: { sec: 8, nanosec: 500 } }
    ]
  }
}"

继续在终端给 my_send_goal.sh 文件添加权限，并执行

touch my_send_goal.sh
chmod +x my_send_goal.sh
./my_send_goal.sh 

6.4.3 效果展示

6.5 上面是直接命令发的，接下来用Moveit 发

同样地，在 src/mybot/launch 路径下（和上面跑的 demo.launch.py 同路径），新建一个文件： my_moveit_rviz.launch.py，并将下面代码直接复制进去保存

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_moveit_rviz_launch

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
)
from moveit_configs_utils.launch_utils import (
    add_debuggable_node,
    DeclareBooleanLaunchArg,
)
from launch.substitutions import LaunchConfiguration
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("six_arm", package_name="mybot").to_moveit_configs() # 之前不知道为什么写错了，现在修改过来了。 20230612，感谢评论区的指正。

    ld = LaunchDescription()

    # 启动move_group
    my_generate_move_group_launch(ld, moveit_config)
    # 启动rviz
    my_generate_moveit_rviz_launch(ld, moveit_config)

    return ld


def my_generate_move_group_launch(ld, moveit_config):

    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    ld.add_action(
        DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
    )
    ld.add_action(
        DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
    )
    # load non-default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
    # inhibit these default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))

    # do not copy dynamics information from /joint_states to internal robot monitoring
    # default to false, because almost nothing in move_group relies on this information
    ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))

    should_publish = LaunchConfiguration("publish_monitored_planning_scene")

    move_group_configuration = {
        "publish_robot_description_semantic": True,
        "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
        # Note: Wrapping the following values is necessary so that the parameter value can be the empty string
        "capabilities": ParameterValue(
            LaunchConfiguration("capabilities"), value_type=str
        ),
        "disable_capabilities": ParameterValue(
            LaunchConfiguration("disable_capabilities"), value_type=str
        ),
        # Publish the planning scene of the physical robot so that rviz plugin can know actual robot
        "publish_planning_scene": should_publish,
        "publish_geometry_updates": should_publish,
        "publish_state_updates": should_publish,
        "publish_transforms_updates": should_publish,
        "monitor_dynamics": False,
    }

    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
    ]
    move_group_params.append({"use_sim_time": True})

    add_debuggable_node(
        ld,
        package="moveit_ros_move_group",
        executable="move_group",
        commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
        output="screen",
        parameters=move_group_params,
        extra_debug_args=["--debug"],
        # Set the display variable, in case OpenGL code is used internally
        additional_env={"DISPLAY": ":0"},
    )
    return ld

def my_generate_moveit_rviz_launch(ld, moveit_config):
    """Launch file for rviz"""

    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    ld.add_action(
        DeclareLaunchArgument(
            "rviz_config",
            default_value=str(moveit_config.package_path / "config/moveit.rviz"),
        )
    )

    rviz_parameters = [
        moveit_config.planning_pipelines,
        moveit_config.robot_description_kinematics,
    ]
    rviz_parameters.append({"use_sim_time": True})

    add_debuggable_node(
        ld,
        package="rviz2",
        executable="rviz2",
        output="log",
        respawn=False,
        arguments=["-d", LaunchConfiguration("rviz_config")],
        parameters=rviz_parameters,
    )

    return ld

然后 colcon  build 编译，然后在两个终端下运行下面两个命令

ros2 launch mybot gazebo.launch.py
ros2 launch mybot my_moveit_rviz.launch.py 

在这里选择之前添加的 pos1 ，然后Plan & Execute，发现 Rviz 和 gazebo 同时都在动。

四、快速通关，可能也会有问题，若会反馈会及时修改。

        另外，毕竟是助手生成的东西，新手看不懂推荐试试vs code插件 通义灵码 去辅助解读。

        二次开发方向：试试自己的urdf；安装moveit2源码学习机械臂的路径规划和运动控制；进一步将仿真和实际机械臂联调；etc.；