【ROS2】通过launch使用gazebo11打开xacro模型（含机械臂）

ros2刚使用不久，如文章内容有误，欢迎在评论内讨论与指正。

---

环境配置：
ubuntu版本：22.04
ros版本：ros2 humble

功能包目录

rob_gazebo
├── CMakeLists.txt
├── package.xml
├── config
│   ├── initial_positions.yaml
│   ├── joint_limits.yaml
│   ├── kinematics.yaml
│   ├── moveit_controllers.yaml
│   ├── moveit.rviz
│   ├── pilz_cartesian_limits.yaml
│   ├── robot_description.ros2_control.xacro
│   ├── robot_description.srdf
│   ├── robot_description.urdf.xacro
│   └── ros2_controllers.yaml
├── launch
│   ├── demo.launch.py
│   ├── gazebo.launch.py
│   ├── move_group.launch.py
│   ├── moveit_rviz.launch.py
│   ├── rsp.launch.py
│   ├── setup_assistant.launch.py
│   ├── spawn_controllers.launch.py
│   ├── static_virtual_joint_tfs.launch.py
│   └── warehouse_db.launch.py
└── urdf_xacro
    ├── robot_simple_gazebo.urdf.xacro
    └── rob_trans.xacro

这个功能包里的大部分文件是由moveit2生成的（不得不说，moveit2比1生成的文件精简了太多，但是不给生成gazebo相关的文件了，想要联合仿真还是得自己写），需要自己创建的文件是gazebo.launch.py和urdf_xacro下的文件，当然，如果你不使用moveit2，还需要创建ros2_controllers.yaml文件，下面都会详细说。

1、xacro文件

robot_simple_gazebo.urdf.xacro内是从soildworks导出的urdf改的xacro，里面只包含机器人的link和joint。重点在rob_trans.xacro，里面放的是ros2_control和gazebo_ros2_control的东西，是机械臂能立起来，轮子能转起来的原因根本。
看一下rob_trans.xacro里的内容

<robot name="rob_gazebo" xmlns:xacro="http://wiki.ros.org/xacro">
  <ros2_control name="GazeboSystem" type="system">
      <hardware>
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>
      <joint name="joint1">
          <command_interface name="position" />
          <state_interface name="position"/>
          <state_interface name="velocity"/>
      </joint>
      <joint name="wheel1">
          <command_interface name="velocity">
            <param name="min">-10</param>
            <param name="max">10</param>
          </command_interface>
          <state_interface name="position"/>
          <state_interface name="velocity"/>
      </joint>
  </ros2_control>

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

这里主要由两部分组成，ros2_control和gazebo两个标签。
和ros1相比，ros2_control替代了transmission，可以有多种控制器，使用名称空间根据控制器使用的命令接口类型对控制器进行排序，控制器使用通用硬件接口定义（英语渣翻译不太好，详细可以看官网），个人感觉是简单清晰了很多。
interface有三种：position、velocity、effort。你的joint首选用哪个控制器接口，就在command_interface标签里面写哪个，state_interface写一个还是多个我没感受出来哪里不同，大家按需使用吧。
gazebo_ros2_control取代了gazebo_ros_control插件，这里和ros1里不一样的是，在gazebo_ros2_control里面需要指明controllers的yaml文件位置，下面我们就要说这个文件。

2、ros2_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    arm_controller:
      type: position_controllers/JointGroupPositionController
   	  #type: joint_trajectory_controller/JointTrajectoryController

    wheel_controller:
      type: velocity_controllers/JointGroupVelocityController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

arm_controller:
  ros__parameters:
    joints:
      - joint1
    interface_name: position
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
      - 
wheel_controller:
  ros__parameters:
    joints:
      - wheel1
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity

这部分和ros1里面的比较相似，大家根据自己的机器人把controller配置好就行。不过在官方给的示例文件中，位置控制使用的控制器类型都是joint_trajectory_controller/JointTrajectoryController，我自己根据速度和力矩控制的示例文件推出的position_controllers/JointGroupPositionController，大家根据自身需求研究它们的话题和消息格式吧。
此外，joint_state_broadcaster是用来发布机器人关节状态的，会产生/joint_state和/dynamic_joint_state两个话题。如果不给它joints参数，则默认发表controller_manager下所有joint的状态。所以在launch里启动了这个，就不用再启动joint_state_publisher节点了，会造成冲突。

3、gazebo.launch.py

最后就是把它们整合到一起的启动文件啦。这一部分参考的是gazebo_ros2_control官方在github放的示例文件

import os

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

import xacro


def generate_launch_description():
	# 启动gazebo
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
             )
	# 下面这一大串是为了读取xacro
    robot_gazebo_path = os.path.join(get_package_share_directory('rob_gazebo'))

    xacro_file = os.path.join(robot_gazebo_path,
                              'urdf_xacro',
                              'robot_simple_gazebo.urdf.xacro')

    doc = xacro.parse(open(xacro_file))
    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]
    )
	# 把机器人模型加载到gazebo
    spawn_entity = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-topic', 'robot_description','-entity', 'robot_gazebo'],
        output='screen'
    )
    # 下面这几个都是加载启动controller
    load_joint_state_controller = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    load_arm_controller = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "arm_controller",
            "--controller-manager",
            "/controller_manager",
        ],
    )
    
    load_wheel_controller = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "wheel_controller",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    return LaunchDescription([
        gazebo,
        node_robot_state_publisher,
        spawn_entity,
        load_joint_state_controller,
        load_arm_controller,
        load_gripper_controller,
        load_wheel_controller
    ])

如果直接复制，记得将里面的注释删去。这里没什么特别要说明的，注意把文件路径写对，然后把所有controller都加载启动。

4、遇到的问题

因为我的机器人是包含机械臂的，所以参考了这篇文章ROS2中，从SolidWorks导出的urdf，联合moveit、gazebo进行控制及仿真，里面提到了

过了一段时间后，会发现机械手的各个零件各自脱离，漫天飞

这个问题我也遇到了，但是我的更离谱，我的机械臂会慢慢散架，然后突然带着我的车体弹飞。原文给出的解释是从soildworks导出会有机会遇到惯性参数有误的bug，这是解决方案。但是因为我这模型文件是从ros1直接拿过来的，当时在ros1下运行的时候没有出过散架的问题，而且我的机器人零件有些多，一个个重新算太麻烦了，于是我在这里找到了我现在的解决方案——向我的机械臂joint标签下加入<dynamics damping="0.7" friction="1.0"/>，翻了一下panda机械臂的urdf文件，也是加了这两个参数的，应该没有什么问题，数值可以根据情况酌情修改。

---

更新：抽空研究了一下惯性矩阵的东西，之前给的那个解决方案写的不是很详细，推荐大家看这一篇从solidworks计算惯性矩阵的文章，讲的很详细，具体的操作步骤都有，不过要补充说明一点，在我使用的过程中，从solidworks得到的惯性矩阵，有一部分link要给非对角线上的数值取反（正的改成负的，负的改成正的，参考链接），然后再用，否则在gazebo-view-inertia里显示的角度是合不上模型的位置的。但是有一些link就不用取反，这是为什么呀？