MoveIt2学习笔记（一）：MoveIt + Gazebo + RViz 的连接

系统ubuntu22.04
ros2 humble

学习教程 韭菜钟 的 在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手。

主要目的是学习MoveIt + Gazebo + RViz 之间的连接方法：实践和原理。

韭菜钟 教程写的非常详细，我这里记录我遇到的问题和解决方法。

2.机械手urdf文件的编写（我没问题）

urdf原理比较简单，他是基于xml的语法，比较好理解，主要就是建立link和joint，格式也很清晰。注意xml文件的第一行不要有空格，否则报无法解析的错。

2.1 <gazebo>标签

但是，这个语法是我第一次见到：

这段代码含义：

	<gazebo reference="base_link"> <!-- 为 base_link 设置 Gazebo 仿真相关的属性 --> 
		<material>Gazebo/Black</material> <!-- 指定 base_link 的材质为黑色 --> 
		<gravity>true</gravity> <!-- base_link 将受到重力的影响 --> 
		<selfCollide>false</selfCollide> <!-- base_link 不与机器人自身的其他连杆发生碰撞 --> 
	</gazebo>

注释解析：
reference="link_name": 指定该 Gazebo 配置作用于哪个连杆（link_name）。
<material>: 定义该连杆在 Gazebo 仿真中的视觉材质（颜色或纹理），这里使用的是 Gazebo 内置材质（例如 Gazebo/Black）。
<gravity>: 是否允许该连杆受到重力影响。默认值是 true。
<selfCollide>: 设置该连杆是否与机器人自身的其他连杆进行碰撞检测。默认值是 false。

原理如下：

2.2. <gazebo>标签扩展

ros2 的 urdf文件中与gazebo设置相关的语法解析
详情查看官方文档 Gazebo中的URDF

在 ROS 2 的 URDF 文件中，与 Gazebo 仿真相关的设置通常通过 <gazebo> 标签进行扩展。这些标签不是标准 URDF 的一部分，而是 Gazebo 插件提供的扩展，用于定义机器人在仿真中的物理属性、传感器、控制器、gazebo插件等。只有在gazebo仿真才需要。

以下是常见的语法和解析：

1. 定义 Gazebo 扩展
   <gazebo> 标签用来指定 Gazebo 特定的仿真参数，可以放在 <robot> 或 <link> 或<joint>标签内。

   <robot name="example_robot">
   	<gazebo>
   		<!-- Gazebo 全局设置 -->
   	</gazebo>
   </robot>
   

2. 机器人全局参数
   可以在 <robot> 的 <gazebo> 标签中定义全局属性：

   <robot name="example_robot">
   	<gazebo>
   		<static>false</static> <!-- 指定机器人是否静止。如果为 true，机器人将是固定的，不会受重力或力的作用。 -->
   		<self_collide>true</self_collide> <!-- 是否启用自碰撞 -->
   		<enable_wind>false</enable_wind> <!-- 是否启用风的影响 -->
   	</gazebo>
   </robot>
   

3. 定义连杆的 Gazebo 参数
   为 <link> 添加 Gazebo 相关属性，比如碰撞、惯性、视觉等。

   <link name="base_link">
   	<visual>
   	<geometry>
   		<box size="1 1 1"/>
   		</geometry>
   	</visual>
   	<gazebo>
   		<material>Gazebo/Red</material> <!-- 指定 Gazebo 中的视觉材质 -->
   		<gravity>true</gravity> <!-- 控制该连杆是否受重力影响 -->
   	</gazebo>
   </link>
   

4. 关节控制设置
   为 <joint> 配置传感器或控制插件。

   <joint name="joint1" type="revolute">
   	<parent link="base_link"/>
   	<child link="link1"/>
   	<gazebo>
   		<dynamics> <!-- 定义关节的动态特性（例如阻尼和摩擦）。 -->
   			<damping>0.1</damping> <!-- 设置关节的阻尼系数。 -->
   		</dynamics>
   	</gazebo>
   </joint>
   

5. 插件定义
   通过 <plugin> 标签加载 Gazebo 插件来模拟传感器或控制器。

   <gazebo>
   	<plugin name="joint_position_controller" filename="libgazebo_ros_joint_position_controller.so">
   		<joint>joint1</joint> <!-- 控制的关节名称 -->
   		<ros>
   			<namespace>/robot_namespace</namespace> <!-- ROS 命名空间 -->
   		</ros>
   	</plugin>
   </gazebo>
   

   说明
   <plugin>: 定义插件名称和对应的 Gazebo 插件库文件。
   <joint>: 指定插件作用的关节。
   <ros>: ROS 相关的设置（如命名空间）。

6. 碰撞和摩擦参数
   为连杆定义碰撞特性。

   <collision>
   	<geometry>
   		<box size="1 1 1"/>
   	</geometry>
   	<surface>
   		<friction>
   			<ode>
   				<mu>1.0</mu> <!-- 动摩擦系数 -->
   				<mu2>1.0</mu2> <!-- 滑动方向的摩擦系数 -->
   			</ode>
   		</friction>
   		<bounce>
   			<restitution_coefficient>0.1</restitution_coefficient> <!-- 恢复系数 -->
   		</bounce>
   	</surface>
   </collision>
   

   说明
   <mu> 和 <mu2>: 定义摩擦系数。
   <restitution_coefficient>: 定义碰撞的弹性系数。

7. 惯性参数自动生成
   如果没有明确定义惯性矩，Gazebo 会根据形状和密度自动生成。
   手动定义惯性参数

   <inertial>
   	<mass value="1.0"/>
   	<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
   </inertial>
   

8. 传感器模拟
   通过 <sensor> 标签定义 Gazebo 中的传感器。

   <gazebo>
   	<sensor type="camera" name="camera_sensor">
   		<update_rate>30.0</update_rate>
   		<camera>
   			<horizontal_fov>1.047</horizontal_fov>
   			<image>
   				<width>640</width>
   				<height>480</height>
   				<format>R8G8B8</format>
   			</image>
   		</camera>
   	</sensor>
   </gazebo>
   

   说明
   <update_rate>: 传感器更新频率。
   <image>: 设置相机分辨率和图像格式。

总结

1. 以上是 URDF 中与 Gazebo 仿真相关的语法解析和示例。如果需要更加复杂的仿真行为，可以结合 SDF（Simulation Description Format）文件或通过 ROS 2 的 Gazebo 插件扩展功能。

2. 添加Gazebo控制器插件，Gazebo插件可以根据插件的作用范围应用到URDF模型的<robot>、<link>、<joint>上，需要使用<gazebo>标签封装

   <!---->
   <!--为<robot>元素添加Gazebo插件-->
   <gazebo>
       <!--name是自己随便定义的控制器插件名字，filename是你要去调用哪一个控制器插件-->
       <plugin name="unique_name" filename="plugin_name.so">
           ...plugin parameters...
       </plugin>
   </gazebo>
    
   <!--为<link>、<joint>标签添加插件-->
   <gazebo reference="your_link_name">
       <plugin name="unique_name" filename="plugin_name.so">
           ...plugin parameters...
       </plugin>
   </gazebo>
   

2.3 <ros2_control name="GazeboSystem" type="system">

这个语法也是我第一次见到：
这段代码含义：

<!--
在运行 demo.launch.py 时，需要注释掉此 ros2_control 节点。
原因是 demo 使用了 xxx.ros2_control.xacro 文件动态生成了 ros2_control 节点，
如果此节点未注释，则会导致重复定义的错误。
-->
<!-- <ros2_control>: 定义一个 ROS 2 控制节点，指定硬件插件以及关节控制和状态接口。 -->
<!-- 定义一个 ros2_control 系统接口，名称为 GazeboSystem -->
<ros2_control name="GazeboSystem" type="system">
	<!-- `<hardware>`: 硬件配置部分，通过指定插件来将 Gazebo 与 ROS 2 控制框架集成。 -->
	<hardware>
		<!-- 指定 Gazebo 使用的 ros2_control 插件，用于连接 Gazebo 和 ROS 2 控制框架 -->
		<plugin>gazebo_ros2_control/GazeboSystem</plugin>
	</hardware>
	
	<!-- 定义 joint1 的控制和状态接口 -->
	<joint name="joint1">
		<!-- <command_interface>`: 指定控制接口，比如位置（position）、速度（velocity）、力矩（effort）。 -->		
		<!-- 定义 joint1 的命令接口 -->
		<command_interface name="position">
			<!-- 允许 joint1 的位置控制命令范围在 [-1, 1] 之间 -->
			<param name="min">-1</param>  <!-- param min 和 param max 限制关节命令的范围。 -->
			<param name="max">1</param>
		</command_interface>
		
		<!-- <state_interface>: 定义状态接口，描述关节的当前状态（如位置或速度）。 -->
		<!-- 定义 joint1 的状态接口 -->
		<state_interface name="position">
			<!-- 设置 joint1 的初始位置状态值为 0.0 -->
			<param name="initial_value">0.0</param> <!-- param initial_value 指定关节的初始状态值。 -->
		</state_interface>
		
		<!-- 定义 joint1 的速度状态接口（不设置参数，默认初始值为 0） -->
		<state_interface name="velocity"/>
	</joint>
</ros2_control>

提示：
如果运行 demo.launch.py，且使用了 .xacro 文件动态生成了 ros2_control 节点，记得将此段代码注释。
如果需要调试独立的 Gazebo 控制器，则可以直接启用此段代码，避免动态生成。

2.4 <gazebo> 标签的 <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">

还有一段：
这段代码配置了一个 gazebo_ros2_control 插件，它允许 Gazebo 仿真机器人通过 ROS 2 控制器进行操作，并根据指定的参数文件（ros2_controllers.yaml）定义控制器的行为。

解释：

<gazebo>
	<!--	定义一个 Gazebo 插件，用于将 Gazebo 仿真与 ROS 2 控制框架集成。
	此插件通过 ros2_control 接口控制机器人关节或执行器。	-->
	<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
		<!--	指定插件的参数文件路径。
		$(find mybot) 表示找到名为 "mybot" 的包，
		其路径下的 config/ros2_controllers.yaml 文件包含控制器配置。	-->
		<parameters>$(find mybot)/config/ros2_controllers.yaml</parameters>
		
		<!--	指定使用的 ROS 2 节点名称，用于获取机器人模型参数。
		这里的 robot_state_publisher 通常发布 TF 变换和机器人的状态信息。	-->
		<robot_param_node>robot_state_publisher</robot_param_node>
	</plugin>
</gazebo>

4.机械手与Gazebo的关联

4.1 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):
	# 删除 XML 文件中的注释内容
	pattern = r'<!--(.*?)-->'
	return re.sub(pattern, '', text, flags=re.DOTALL)

def generate_launch_description():
	# 定义机器人模型名称和描述包信息
	robot_name_in_model = 'six_arm' # 模型的名称
	package_name = 'mybot_description' # 存放机器人描述文件的 ROS 包名
	urdf_name = "six_arm.urdf" # 机器人的 URDF 文件名称

	# 获取机器人描述包路径，并生成 URDF 文件的完整路径
	pkg_share = FindPackageShare(package=package_name).find(package_name)
	urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

	# 启动 Gazebo 仿真服务
	start_gazebo_cmd = ExecuteProcess(
		cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
		output='screen'
	)

	# 因为 URDF 文件中包含 xacro 的语句，使用 xacro 解析并生成 XML 文件
	xacro_file = urdf_model_path # URDF 文件路径
	doc = xacro.parse(open(xacro_file)) # 解析 xacro 文件
	xacro.process_doc(doc) # 处理 xacro 文件生成完整的 XML
	params = {'robot_description': remove_comments(doc.toxml())} # 去除注释后保存为参数
	
	# 启动 robot_state_publisher 节点
	# 该节点发布机器人描述 (robot_description) 和 TF 信息
	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'
	)
	
	# 在 Gazebo 中生成机器人模型，通过 robot_description 话题提供模型内容
	spawn_entity_cmd = Node(
		package='gazebo_ros',
		executable='spawn_entity.py',
		arguments=['-entity', robot_name_in_model, '-topic', 'robot_description'],
		output='screen'
	)
	
	# 加载关节状态发布器 
	load_joint_state_controller = ExecuteProcess( 
		cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], 
		output='screen' 
	) 
	
	# 加载路径执行控制器 
	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 
	close_evt2 = RegisterEventHandler( 
		event_handler=OnProcessExit( 
			target_action=load_joint_state_controller, 
			on_exit=[load_joint_trajectory_controller], 
		) 
	) 
	
	# 创建 LaunchDescription 对象，用于管理所有启动动作 
	ld = LaunchDescription() 
	
	# 添加事件处理器，确保启动顺序正确 
	ld.add_action(close_evt1) 
	ld.add_action(close_evt2) 
	
	# 添加启动动作 
	ld.add_action(start_gazebo_cmd)     # 启动 Gazebo 仿真服务 
	ld.add_action(node_robot_state_publisher)     # 启动 robot_state_publisher 节点 l
	d.add_action(spawn_entity_cmd)     # 生成机器人模型 
	return ld # 返回完整的启动描述

4.2 ExecuteProcess

文中代码：

	# 启动 Gazebo 仿真服务
	start_gazebo_cmd = ExecuteProcess(
		cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
		output='screen'
	)

这一段代码负责启动 Gazebo 仿真服务，下面是详细解释：

1. ExecuteProcess
   ExecuteProcess 是 ROS 2 的一个动作类，用于启动一个外部进程，在这里用来运行 Gazebo 仿真程序。
   作用：启动 Gazebo 服务器（gazebo 命令）。
   主要参数：cmd: 要执行的命令及其参数。
   output: 定义进程输出方式，这里设置为 screen，将 Gazebo 的输出打印到终端屏幕。
2. cmd 参数详解
   cmd 是一个列表，其中包含要执行的命令及其参数：
   cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so']
   a. gazebo
   作用：启动 Gazebo 仿真环境。Gazebo 是一个 3D 机器人仿真软件，允许用户模拟机器人行为、传感器、物理环境等。
   b. --verbose
   作用：启用详细日志输出。Gazebo 在终端打印更详细的信息（例如加载资源、插件、警告和错误），用于调试仿真过程。
   c. -s
   作用：加载 Gazebo 插件。每个 -s 后面跟一个插件的共享库文件（.so），用来扩展 Gazebo 的功能。
3. 加载的插件
   a. libgazebo_ros_init.so
   作用：将 Gazebo 和 ROS 2 框架连接起来。负责初始化 ROS 2 与 Gazebo 的通信，包括创建 ROS 2 节点和订阅/发布消息。
   功能：使 Gazebo 支持 ROS 2 的时间（/clock 话题）。接收来自 ROS 2 的仿真控制信号（例如暂停、重置仿真等）。
   b. libgazebo_ros_factory.so
   作用：提供一个接口，允许通过 ROS 2 动态生成仿真中的实体（例如机器人、物体等）。
   功能：订阅 ROS 2 话题（如 /gazebo/spawn_entity），根据发送的消息动态加载机器人模型或其他对象到仿真环境中。
4. output='screen'
   作用：将 Gazebo 的输出信息直接打印到终端屏幕。
   包括插件加载信息、仿真状态、错误和警告等，方便用户实时查看和调试。
   综合作用：

这一段代码的作用是启动 Gazebo 仿真服务，并加载两个关键的 ROS 2 插件：
libgazebo_ros_init.so: 用于初始化 ROS 2 与 Gazebo 的通信。
libgazebo_ros_factory.so: 用于支持通过 ROS 2 动态加载模型或实体。
通过 --verbose，用户可以实时获取详细的调试信息，便于排查问题。仿真环境启动后，Gazebo 会通过 ROS 2 提供的接口与其他 ROS 节点协作，例如加载机器人模型、订阅控制器命令等。

4.3 spawn_entity_cmd

这一段代码的作用是通过 ROS 2 节点将机器人模型生成到 Gazebo 仿真环境 中，以下是详细解析：

文中代码：

	spawn_entity_cmd = Node(
		package='gazebo_ros',
		executable='spawn_entity.py',
		arguments=['-entity', robot_name_in_model, '-topic', 'robot_description'],
		output='screen'
	)

逐行解释：

1. Node
   Node 是 ROS 2 启动文件中的一个动作类，用于启动一个 ROS 2 节点。
   在这里，Node 启动的是 spawn_entity.py，它是一个用于将机器人模型动态加载到 Gazebo 中的脚本。
2. package='gazebo_ros'
   作用：指定运行节点所在的 ROS 2 包。
   gazebo_ros 是 Gazebo 提供的 ROS 2 集成包，包含与仿真相关的工具和功能模块。
3. executable='spawn_entity.py'
   作用：指定要执行的脚本或可执行文件的名称。
   spawn_entity.py 是 Gazebo ROS 2 提供的一个脚本，用于通过 ROS 2 动态将实体（如机器人模型）加载到 Gazebo 仿真环境中。
4. arguments
   作用：传递给 spawn_entity.py 脚本的参数，用于控制模型生成的细节。
   参数解释：arguments=['-entity', robot_name_in_model, '-topic', 'robot_description']
-entity:指定实体的名称，即机器人在 Gazebo 仿真环境中的唯一标识。
   robot_name_in_model 是一个变量，通常定义为机器人模型的名称（例如 six_arm）。
   -topic:指定一个 ROS 2 话题，提供实体的描述文件。
   robot_description 是 ROS 2 中的标准话题名称，通常用于发布机器人模型的 URDF/SDF 数据。
5. output='screen'
   作用：将脚本的输出信息（如日志、警告或错误）打印到终端屏幕。
   意义：方便用户实时查看节点运行状态，特别是在加载模型时，可以快速发现和定位错误。

脚本 spawn_entity.py 的工作原理

1. 订阅 robot_description 话题：
   spawn_entity.py 脚本从指定的 ROS 2 话题（如 robot_description）中读取机器人模型的描述文件。
   该文件通过 robot_state_publisher 节点发布，包含 URDF 或 SDF 格式的模型内容。
   解析机器人模型：
   解析从 robot_description 接收到的模型描述数据。
2. 加载机器人模型到 Gazebo：
   调用 Gazebo 插件（如 libgazebo_ros_factory.so），将解析后的机器人模型加载到仿真环境中。
3. 分配实体名称：
   使用 -entity 参数指定的名称（如 six_arm）为机器人模型命名，以便后续在 Gazebo 中识别该模型。

代码综合作用

1. 启动 spawn_entity.py 节点：
   该节点通过 ROS 2 动态加载机器人模型到 Gazebo 仿真环境中。
2. 使用 robot_description 提供模型数据：
   通过 robot_state_publisher 发布的 robot_description 话题，提供机器人模型的完整描述。
3. 在 Gazebo 中生成机器人模型：
   使用 Gazebo 的 ROS 2 插件接口，将机器人模型渲染并加载到仿真世界中。

运行结果

1. Gazebo 仿真环境中会动态生成一个机器人模型，模型的名称为 robot_name_in_model（例如 six_arm）。
2. 用户可以通过 Gazebo 的界面查看加载的机器人，并通过 ROS 2 话题对其进行操作或监控。

4.4 joint_state_broadcaster

文中代码

	# 加载关节状态发布器 
	load_joint_state_controller = ExecuteProcess( 
		cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], 
		output='screen' 
	) 

这一段代码用于 加载关节状态发布器（Joint State Broadcaster）控制器，以下是详细解析：

1. load_joint_state_controller = ExecuteProcess(…)
   ExecuteProcess 是 ROS 2 的一个动作类，用于执行外部命令。
   load_joint_state_controller 是一个变量，表示执行此动作的实例。
2. cmd 参数
   cmd 是要执行的外部命令及其参数列表：
   cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster']
   a. ros2 control load_controller
   作用：ROS 2 控制框架中的命令，用于加载和激活指定的控制器。
   背景：ROS 2 使用控制器（Controllers）来操作机器人硬件或模拟的关节。
   控制器由 ros2_control 框架管理，可以通过命令加载、激活或禁用。
   b. --set-state active
   作用：将控制器的状态设置为 active（激活）。控制器在加载后默认是 inactive 状态，必须设置为 active 才能开始工作。
   c. joint_state_broadcaster
   作用：指定要加载的控制器的名称。
   joint_state_broadcaster 是 ROS 2 的一个标准控制器：
   功能：发布关节状态（joint_states 话题），包括关节位置、速度、和力矩。
   用途：用于监控机器人关节的状态信息，提供给其他节点（如 robot_state_publisher）以发布 TF 信息。

控制器 joint_state_broadcaster 的作用

1. 状态发布：
   监听机器人硬件接口或仿真接口中的关节状态数据。
   将这些数据以标准的 ROS 2 话题 joint_states 发布。
2. 数据用途：
   TF 发布：robot_state_publisher 节点通过订阅 joint_states，根据关节状态计算并发布机器人各部分的 TF（坐标变换）。
3. 运动监控：其他节点可以订阅 joint_states，获取机器人关节实时状态用于分析或反馈控制。

综合作用

1. 这段代码通过 ros2 control 命令，动态加载并激活 关节状态发布器控制器，以便在仿真或硬件环境中实时发布关节状态，为后续的 TF 计算和机器人监控提供必要的数据支持。

运行结果

1. joint_state_broadcaster 成功加载并激活。
2. joint_states 话题开始发布关节状态，包括：
   关节位置（position）
   关节速度（velocity）
   关节力矩（effort）

4.5 my_group_controller

前面的解释和4.4一样，不同的是这里的my_group_controller并不是ros2 control的标准控制器，那么系统是如何找到my_group_controller控制器的呢？
c. my_group_controller
作用：指定要加载的控制器名称。 这里my_group_controller是路径执行控制器的名称，需要在 ros2_controllers.yaml 配置文件中定义。
Joint Trajectory Controller：一个标准控制器，接受轨迹命令并执行关节的同步运动。 通过话题（ /joint_trajectory_controller/follow_joint_trajectory）订阅路径命令。

代码的 ros2_controllers.yaml 文件为在moveit_setup_assistant配置后自动生成，不用修改，直接使用，如下：

# This config file is used by ros2_control
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    my_group_controller:	# 控制器的名称
      type: joint_trajectory_controller/JointTrajectoryController # 控制器类型
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
my_group_controller:
  ros__parameters: # 控制器参数
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    command_interfaces:
      - position # 控制接口类型（如位置、速度、力矩等）
    state_interfaces:
      - position
      - velocity
    allow_nonzero_velocity_at_trajectory_end: true

盲猜my_group_controller和joint_state_broadcaster都是在这里定义的新控制器~~

路径执行控制器的作用

1. 路径执行：
   订阅路径命令话题（如 /follow_joint_trajectory），接收机器人关节的目标轨迹。
   将轨迹解析为多个关节的运动控制命令。
   根据目标轨迹时间，控制关节以同步的方式完成运动。
2. 关键功能：
   提供精准的多关节同步控制。
   支持从 MoveIt 或其他运动规划节点发送路径命令。

综合作用

1. 这段代码通过 ros2 control 命令加载并激活路径控制器（Joint Trajectory Controller）。控制器的主要功能是执行机器人关节的同步轨迹运动，通常由运动规划框架（如 MoveIt）生成轨迹并发送命令给该控制器。

运行结果

1. my_group_controller 控制器加载成功： 其状态变为 active，可以接收轨迹命令。
2. 路径执行功能开启：
   控制器开始监听路径命令（如 /follow_joint_trajectory 话题）。
   机器人关节根据路径命令以时间为基准完成目标位置的运动。

4.6 RegisterEventHandler

文中代码：

	# 监听 spawn_entity_cmd 的退出事件，随后启动 load_joint_state_controller 
	close_evt1 = RegisterEventHandler( 
		event_handler=OnProcessExit( 
			target_action=spawn_entity_cmd, 
			on_exit=[load_joint_state_controller], 
		) 
	)	

这段代码设置了一个 事件监听器，用于确保在 spawn_entity_cmd 动作完成（退出）后，启动 load_joint_state_controller 动作。以下是详细解析：

1. close_evt1 = RegisterEventHandler(...)
RegisterEventHandler：用于注册一个事件监听器。
   监听器会监视指定的事件，并在事件发生时触发后续的操作。
   close_evt1：变量名称，用于存储这个注册的事件监听器实例。
   该监听器监听 spawn_entity_cmd 的退出事件，并启动 load_joint_state_controller。
2. event_handler=OnProcessExit(...)
OnProcessExit：是一个具体的事件处理器，用于监听某个进程（或动作）的退出事件。
   当指定的动作退出时（即动作完成），触发定义的操作。
   参数解析：target_action=spawn_entity_cmd：表示要监听的目标动作是 spawn_entity_cmd。
   也就是说，当 spawn_entity_cmd 动作退出（机器人模型生成完成）时，触发后续操作。
   on_exit=[load_joint_state_controller]：定义在目标动作退出后需要执行的动作。
   这里是启动关节状态控制器 load_joint_state_controller。
3. target_action=spawn_entity_cmd
spawn_entity_cmd：一个动作实例，用于通过 spawn_entity.py 将机器人模型加载到 Gazebo 中。
   此处监听的是 spawn_entity_cmd 的退出事件，即机器人模型加载完成的时刻。
4. on_exit=[load_joint_state_controller]
   作用：定义在监听的动作退出时需要执行的后续操作。
   在这里，当 spawn_entity_cmd 完成后，启动关节状态控制器 load_joint_state_controller。

监听器逻辑：监听 spawn_entity_cmd 动作的退出事件。

1. 当 spawn_entity_cmd 动作完成（即机器人模型加载到 Gazebo 仿真中）后，触发 load_joint_state_controller 动作。
2. 目的：确保机器人模型已经成功加载到仿真环境后，再加载关节状态发布器控制器（joint_state_broadcaster）。
   避免控制器在模型尚未生成时启动，导致错误。

运行流程

• spawn_entity_cmd 动作启动，开始在 Gazebo 中生成机器人模型。
• 一旦模型生成完成，spawn_entity_cmd 动作退出。
• 监听器捕捉到退出事件，立即启动 load_joint_state_controller 动作。
• load_joint_state_controller 动作加载并激活 joint_state_broadcaster 控制器，开始发布关节状态。

相关应用场景

• 控制启动顺序：机器人模型必须先加载完成，控制器才能正确初始化，否则可能出现错误。
• 避免依赖冲突：如果关节状态控制器在模型未加载时启动，可能导致无法找到关节状态接口的错误。

总结

• 通过 RegisterEventHandler 和 OnProcessExit，确保了 load_joint_state_controller 的启动严格依赖于 spawn_entity_cmd 的完成。这种事件驱动的方式在 ROS 2 启动流程中非常常见，用于处理节点和动作之间的依赖关系和顺序控制。

4.7 LaunchDescription()

原文代码

# 创建 LaunchDescription 对象，用于管理所有启动动作 
	ld = LaunchDescription() 
	
	# 添加事件处理器，确保启动顺序正确 
	ld.add_action(close_evt1) 
	ld.add_action(close_evt2) 
	
	# 添加启动动作 
	ld.add_action(start_gazebo_cmd)     # 启动 Gazebo 仿真服务 
	ld.add_action(node_robot_state_publisher)     # 启动 robot_state_publisher 节点 l
	d.add_action(spawn_entity_cmd)     # 生成机器人模型 
	return ld # 返回完整的启动描述

详细解析：

1. ld = LaunchDescription()
LaunchDescription：ROS 2 中用于定义启动文件的核心对象。
   它管理一系列启动动作和事件，例如启动进程、设置参数、注册监听器等。
   ld：变量名称，用于存储 LaunchDescription 实例。
   它包含所有需要在启动时执行的动作和事件。
2. ld.add_action(close_evt1) 和 ld.add_action(close_evt2)
   添加事件处理器：close_evt1 和 close_evt2 是事件处理器，用于控制动作之间的启动顺序。
   功能：close_evt1：监听 spawn_entity_cmd（生成机器人模型动作）的退出事件。
   在模型生成完成后，启动关节状态控制器 load_joint_state_controller。
   close_evt2：监听 load_joint_state_controller 的退出事件。
   在关节状态控制器激活后，启动路径控制器 load_joint_trajectory_controller。
   目的：确保动作之间的依赖关系和启动顺序。
3. 添加启动动作
   以下代码添加具体的启动动作到 LaunchDescription 中：
   a. ld.add_action(start_gazebo_cmd)
   启动 Gazebo 仿真服务。start_gazebo_cmd 是一个 ExecuteProcess 动作，用于启动 Gazebo 并加载 ROS 2 插件（如 libgazebo_ros_init.so 和 libgazebo_ros_factory.so）。
   b. ld.add_action(node_robot_state_publisher)
   启动 robot_state_publisher 节点。
   功能：发布机器人描述（robot_description）和 TF 信息。用于后续生成机器人模型和计算坐标变换。
   c. ld.add_action(spawn_entity_cmd)
   生成机器人模型。
   功能：使用 spawn_entity.py 脚本，将通过 robot_description 提供的模型加载到 Gazebo 仿真环境中。模型名称为 robot_name_in_model（例如 six_arm）。
4. return ld
   作用：返回完整的 LaunchDescription 对象。
   启动文件的 generate_launch_description() 函数必须返回一个 LaunchDescription 实例，它定义了启动流程。
   包含内容：所有的启动动作（如 start_gazebo_cmd、spawn_entity_cmd）。
   所有的事件处理器（如 close_evt1、close_evt2）。
   启动流程：当运行此启动文件时，ROS 2 会按照 LaunchDescription 中的动作和事件顺序执行。
   运行流程总结
   启动 Gazebo 仿真环境：通过 start_gazebo_cmd 启动 Gazebo，并加载必要的 ROS 2 插件。
   启动 robot_state_publisher 节点：发布 robot_description 和 TF 信息。
   生成机器人模型：通过 spawn_entity_cmd 将机器人模型加载到 Gazebo。
   依次加载控制器：使用事件处理器 close_evt1 和 close_evt2，确保在模型生成后，依次加载关节状态发布器和路径控制器。

代码作用

1. 将启动 Gazebo 仿真环境、加载机器人模型、启动必要的控制器和功能节点的流程组织起来，并确保按正确的顺序执行。
2. 返回的 LaunchDescription 是启动文件的核心输出，供 ROS 2 启动系统使用。

ld.add_action中的顺序换了会产生什么后果呢？

1. 如果启动的动作或节点彼此之间没有显式依赖关系（如事件触发、参数共享等），ld.add_action 的顺序调整不会影响最终结果。
2. 当动作之间存在依赖关系（如事件处理器、节点间参数依赖），顺序的改变可能会导致报错或无法实现代码功能。

5. MoveIt与Gazebo的关联

5.1 my_moveit_rviz.launch.py 代码分析

文件路径

/home/xj/ws_myRobot/src/mybot/launch/my_moveit_rviz.launch.py

代码注释

# 导入 MoveIt 的配置工具类，用于生成机器人配置
from moveit_configs_utils import MoveItConfigsBuilder
# 导入用于生成 RViz 启动文件的工具函数
from moveit_configs_utils.launches import generate_moveit_rviz_launch

# 导入 ROS 2 启动文件的核心类和动作
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,  # 声明启动参数
    IncludeLaunchDescription,  # 包含其他启动文件
)

# 导入 MoveIt 的工具函数，例如调试节点和布尔启动参数
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():
    # 使用 MoveItConfigsBuilder 构建 MoveIt 的配置，指定机器人名称和包名
    moveit_config = MoveItConfigsBuilder("six_arm", package_name="mybot").to_moveit_configs()

    # 创建 LaunchDescription 对象，用于管理所有的启动动作
    ld = LaunchDescription()

    # 启动 move_group 节点
    my_generate_move_group_launch(ld, moveit_config)
    # 启动 RViz 节点
    my_generate_moveit_rviz_launch(ld, moveit_config)

    # 返回包含所有启动动作的描述
    return ld

# 自定义函数，用于生成 move_group 的启动动作
def my_generate_move_group_launch(ld, moveit_config):

    # 声明布尔型启动参数，用于是否启用调试模式，默认值为 False
    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    # 声明布尔型启动参数，用于是否允许路径执行，默认值为 True
    ld.add_action(
        DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
    )
    # 声明布尔型启动参数，是否发布监控的规划场景，默认值为 True
    ld.add_action(
        DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
    )
    # 声明字符串型启动参数，用于加载非默认的 MoveGroup 功能
    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
    # 声明字符串型启动参数，用于禁用默认的 MoveGroup 功能
    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))

    # 声明布尔型启动参数，是否监控关节动态信息，默认值为 False
    ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))

    # 获取是否发布监控的规划场景的参数值
    should_publish = LaunchConfiguration("publish_monitored_planning_scene")

    # 定义 move_group 节点的参数
    move_group_configuration = {
        # 发布语义机器人描述
        "publish_robot_description_semantic": True,
        # 是否允许路径执行，由启动参数控制
        "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
        # 加载的 MoveGroup 功能，由启动参数控制
        "capabilities": ParameterValue(
            LaunchConfiguration("capabilities"), value_type=str
        ),
        # 禁用的 MoveGroup 功能，由启动参数控制
        "disable_capabilities": ParameterValue(
            LaunchConfiguration("disable_capabilities"), value_type=str
        ),
        # 发布与物理机器人相关的规划场景，用于 RViz 插件
        "publish_planning_scene": should_publish,
        "publish_geometry_updates": should_publish,
        "publish_state_updates": should_publish,
        "publish_transforms_updates": should_publish,
        # 默认不监控动态信息
        "monitor_dynamics": False,
    }

    # 将 MoveIt 配置和 move_group 特定配置合并
    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
    ]
    # 添加仿真时间支持的参数
    move_group_params.append({"use_sim_time": True})

    # 向启动描述中添加 move_group 节点，并启用调试选项
    add_debuggable_node(
        ld,
        package="moveit_ros_move_group",  # MoveIt 的 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"],  # 额外的调试参数
        # 设置 DISPLAY 环境变量，确保内部的 OpenGL 代码可以正常工作
        additional_env={"DISPLAY": ":0"},
    )
    return ld  # 返回更新后的 LaunchDescription 对象

# 自定义函数，用于生成 RViz 的启动动作
def my_generate_moveit_rviz_launch(ld, moveit_config):
    """Launch file for RViz"""

    # 声明布尔型启动参数，用于是否启用调试模式，默认值为 False
    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    # 声明字符串型启动参数，指定 RViz 的配置文件路径
    ld.add_action(
        DeclareLaunchArgument(
            "rviz_config",
            default_value=str(moveit_config.package_path / "config/moveit.rviz"),
        )
    )

    # 定义 RViz 节点的参数
    rviz_parameters = [
        moveit_config.planning_pipelines,  # 规划管道的配置
        moveit_config.robot_description_kinematics,  # 运动学参数
    ]
    # 添加仿真时间支持的参数
    rviz_parameters.append({"use_sim_time": True})

    # 向启动描述中添加 RViz 节点，并启用调试选项
    add_debuggable_node(
        ld,
        package="rviz2",  # RViz 2 的包名
        executable="rviz2",  # 可执行文件名
        output="log",  # 将日志输出到日志文件
        respawn=False,  # 如果节点崩溃，不会自动重启
        arguments=["-d", LaunchConfiguration("rviz_config")],  # 使用指定的 RViz 配置文件
        parameters=rviz_parameters,  # 参数列表
    )

    return ld  # 返回更新后的 LaunchDescription 对象

5.2 报错

第5步，在RViz中可以plan，但是execute报错，gazebo也无法跟随运动。。。终端报错是：

[move_group-1] [WARN] [1734753790.939876987] [move_group.moveit.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[move_group-1] [INFO] [1734753790.940145136] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 0 controllers in list
[move_group-1] [INFO] [1734753790.940164790] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 0 controllers in list
[move_group-1] [INFO] [1734753790.940180125] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 0 controllers in list
[move_group-1] [ERROR] [1734753790.940197983] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Unable to identify any set of controllers that can actuate the specified joints: [ joint1 joint2 joint3 joint4 joint5 joint6 ]
[move_group-1] [ERROR] [1734753790.940207265] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Known controllers and their joints:
[move_group-1]
[move_group-1] [ERROR] [1734753790.940224423] [move_group.moveit.moveit.ros.add_time_optimal_parameterization]: Apparently trajectory initialization failed
[move_group-1] [INFO] [1734753790.940343263] [move_group.moveit.moveit.ros.move_group.move_action]: CONTROL_FAILED
[rviz2-2] [INFO] [1734753790.940708481] [moveit_4002535917.moveit.ros.move_group_interface]: Plan and Execute request aborted
[rviz2-2] [ERROR] [1734753790.941020911] [moveit_4002535917.moveit.ros.move_group_interface]: MoveGroupInterface::move() failed or timeout reached

解决方案是在默认生成的moveit_controllers.yaml后面加上两行

    action_ns: follow_joint_trajectory
    default: true

文件路径：

/home/xj/ws_myRobot/src/mybot/config/moveit_controllers.yaml

添加后的完整moveit_controllers.yaml

# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
  controller_names:
    - my_group_controller

  my_group_controller:
    type: FollowJointTrajectory
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    action_ns: follow_joint_trajectory
    default: true

与 MoveIt 控制器的交互方式

1. action_ns: follow_joint_trajectory
   定义：
   action_ns 指定了 MoveIt 控制器与机器人控制器（如硬件控制器或模拟器）通信的 ROS 动作（Action）命名空间。
   MoveIt 的 MoveItSimpleControllerManager 插件会在此命名空间上发送 FollowJointTrajectory 类型的轨迹指令。
   交互流程：
   路径规划完成后：MoveIt 根据规划的路径轨迹，生成目标关节的轨迹点。
   发送轨迹指令：MoveIt 控制器通过 follow_joint_trajectory 命名空间，调用机器人控制器的 ROS 动作服务器。
   动作服务器需要监听该命名空间，处理并执行轨迹指令。
   反馈和结果：机器人控制器通过动作反馈（feedback）和结果（result）接口，通知 MoveIt 轨迹执行的实时状态和最终结果。
   关键点：
   动作类型：FollowJointTrajectory 是 ROS 标准动作接口类型，要求机器人控制器支持该类型的动作接口。
   命名空间一致性：MoveIt 和机器人控制器必须使用相同的命名空间（follow_joint_trajectory）。
2. default: true
   定义：
   表示该控制器为默认控制器。
   如果在规划时未指定具体的控制器，MoveIt 会自动选择此控制器执行轨迹。
   交互流程：
   在复杂系统中，可能存在多个控制器（如机械臂和抓手分别有独立控制器）。
   MoveIt 通过 default: true 知道哪一个控制器是默认选择，用于处理未指定控制器的任务。
   如果任务中明确指定了控制器名称，则不使用default: true 。

如果没有配置这些字段

1. 没有 action_ns
   后果：MoveIt 无法找到控制器的 ROS 动作命名空间。
   轨迹规划完成后，轨迹指令无法发送到机器人控制器，导致路径无法执行。
   解决方法：必须为控制器提供正确的命名空间，确保与机器人控制器的 ROS 动作服务器一致。
2. 没有 default: true
   后果：如果系统中存在多个控制器，且未在任务中明确指定控制器，MoveIt 将不知道使用哪个控制器。
   可能导致任务执行失败或行为不可预测。
   解决方法：确保至少有一个控制器设置为默认控制器，尤其是在单控制器系统中。

后记：结果展示

• gazebo 中 机器人的初始位置
  * gazebo 中 plan & execute 成功