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 将不知道使用哪个控制器。
   可能导致任务执行失败或行为不可预测。
   解决方法：确保至少有一个控制器设置为默认控制器，尤其是在单控制器系统中。

6. 成功的结果展示（韭菜种教程）

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

7. 失败的问题解决（我自己的机器人）

gazebo打不开，卡在这个橙色页面上。。。
终端报错：

[gazebo-1] [Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
[ros2-4] [INFO] [1734791929.373109748] [_ros2cli_63402]: waiting for service /controller_manager/load_controller to become available…
[ros2-4] [WARN] [1734791939.390468368] [_ros2cli_63402]: Could not contact service /controller_manager/load_controller
[ros2-4] [INFO] [1734791939.392110795] [_ros2cli_63402]: waiting for service /controller_manager/load_controller to become available…

ctrl+c终止程序的提示如下：

文字：

^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[robot_state_publisher-2] [INFO] [1735113129.221482096] [rclcpp]: signal_handler(signum=2)
[INFO] [robot_state_publisher-2]: process has finished cleanly [pid 10364]
[ERROR] [ros2-4]: process has died [pid 10488, exit code 2, cmd ‘ros2 control load_controller --set-state active joint_state_broadcaster’].
[gazebo-1] [Msg] Waiting for model database update to complete…
[gazebo-1] [Wrn] [FuelModelDatabase.cc:313] URI not supported by Fuel [model://ok3/meshes/Link_1.STL]
[gazebo-1] [Wrn] [SystemPaths.cc:459] File or path does not exist [“”] [model://ok3/meshes/Link_1.STL]
[gazebo-1] [Err] [Visual.cc:2956] No mesh specified
[ERROR] [gazebo-1]: process[gazebo-1] failed to terminate ‘5’ seconds after receiving ‘SIGINT’, escalating to ‘SIGTERM’
[INFO] [gazebo-1]: sending signal ‘SIGTERM’ to process[gazebo-1]
[ERROR] [gazebo-1]: process has died [pid 10362, exit code -15, cmd ‘gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so’].
[gazebo-1]

ps. ctrl+c后gazebo关不掉，一直卡住，如下命令成功杀死gazebo的进程

killall -9 gzserver gzclient

7.1 有请chatgpt：

从您提供的日志来看，存在以下关键信息：

1. 实体生成成功：

   [spawn_entity.py-3] [INFO] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [ok3]
   

   说明实体（例如机器人模型）已成功加载到Gazebo中，spawn_entity进程完成并正常退出。
2. Gazebo初始化成功：

   [gazebo-1] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
   [gazebo-1] [Msg] Loading world file [/usr/share/gazebo-11/worlds/empty.world]
   

   Gazebo已连接到主控节点，并正确加载了empty.world环境。
3. 加载模型数据库警告：

   [gazebo-1] [Wrn] [ModelDatabase.cc:340] Getting models from [http://models.gazebosim.org/]. This may take a few seconds.
   

   这是一个常见的提示，Gazebo在尝试从远程模型数据库下载模型。如果网络较慢或不可用，这可能会影响加载时间。
4. 控制器管理服务问题： /controller_manager/load_controller服务不可用

   [ros2-4] [INFO] [1735113110.938186801] [_ros2cli_10488]: waiting for service /controller_manager/load_controller to become available...
   [ros2-4] [WARN] [1735113120.955805132] [_ros2cli_10488]: Could not contact service /controller_manager/load_controller
   

   尽管Gazebo和实体生成工作正常，但控制器管理器的/controller_manager/load_controller服务无法联系。这是主要问题。
   说明：ros2 control尝试调用/controller_manager/load_controller服务，但该服务未能启动。
   可能原因：
   ① controller_manager未正确启动。
   ② 控制器配置文件（通常是ROS 2参数文件）不正确。

可能原因及解决方案：

1. 控制器管理器未正确启动
   尽管ros2 service list显示了/controller_manager/load_controller服务，但服务可能未完全初始化或在加载过程中遇到了问题。
   检查controller_manager节点是否在运行：

   ros2 node list
   

   查看是否有名为controller_manager的节点。如果没有启动，请检查您的启动文件是否正确配置了controller_manager。
2. 缺少硬件接口或控制器插件
   如果Gazebo中的机器人模型未正确加载控制器插件，controller_manager将无法正常运行。
   确认URDF或SDF文件中是否正确添加了Gazebo ROS控制插件，例如：

   <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
   	<robotNamespace>/robot_name</robotNamespace>
   </plugin>
   

   确保机器人硬件接口（如hardware_interface）和控制器配置文件已正确设置。
3. 实体生成与服务启动的时序问题
   实体生成完成后，controller_manager可能需要一些时间初始化服务。如果您的代码立即尝试加载控制器，可能会导致找不到服务。
   您可以尝试在代码中加入延迟，等待/controller_manager/load_controller服务变为可用：

   sleep 5
   

4. 调试服务
   手动调用/controller_manager/load_controller服务，查看是否存在更详细的错误信息：

   ros2 service call /controller_manager/load_controller controller_manager_msgs/srv/LoadController "{name: 'controller_name'}"
   

   如果服务调用失败，请进一步检查日志或配置。

逐步尝试chatgpt的解决方案

xj@xj:~$ ros2 node list
/gazebo
/robot_state_publisher
xj@xj:~$ ros2 run controller_manager spawner joint_state_broadcaster
[WARN] [1735114476.343109473] [spawner_joint_state_broadcaster]: Controller already loaded, skipping load_controller
[ERROR] [1735114476.345189090] [spawner_joint_state_broadcaster]: Failed to configure controller
[ros2run]: Process exited with failure 1
xj@xj:~$ ros2 param list /controller_manager
  joint_state_broadcaster.params_file
  joint_state_broadcaster.type
  manipulator_controller.params_file
  manipulator_controller.type
  qos_overrides./clock.subscription.depth
  qos_overrides./clock.subscription.durability
  qos_overrides./clock.subscription.history
  qos_overrides./clock.subscription.reliability
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  robot_description
  update_rate
  use_sim_time
xj@xj:~$ ros2 param get /controller_manager joint_state_broadcaster.params_file
Parameter not set.
xj@xj:~$ ros2 param get /controller_manager joint_state_broadcaster.type
String value is: joint_state_broadcaster/JointStateBroadcaster
xj@xj:~$ cd ws_ok
xj@xj:~/ws_ok$ touch joint_state_broadcaster.yaml
xj@xj:~/ws_ok$ ros2 param set /controller_manager joint_state_broadcaster.params_file ~/ws_ok/joint_state_broadcaster.yaml
Set parameter successful
xj@xj:~/ws_ok$ ros2 param get /controller_manager joint_state_broadcaster.params_file
String value is: /home/xj/ws_ok/joint_state_broadcaster.yaml
xj@xj:~/ws_ok$ 

7.2 查看运行的 ROS 2 节点：ros2 node list

xj@xj:~$ ros2 node list
/gazebo
/robot_state_publisher

从 ros2 node list 的输出可以看到，目前运行的 ROS 2 节点只有以下两个：
① /gazebo：用于与 Gazebo 仿真环境进行通信的节点。
② /robot_state_publisher：发布机器人关节状态的节点。

问题分析

1. controller_manager 未运行：
   ① 由于输出中没有看到 controller_manager 节点，说明控制器管理器并未启动。这是导致 /controller_manager/load_controller 服务不可用的直接原因。
   ② 如果需要控制器运行（如 joint_state_broadcaster 或其他控制器），必须先启动 controller_manager 节点。
2. 节点数量过少：
   ① 目前只有两个节点运行，表明启动的功能有限。通常在复杂仿真中会涉及更多节点，如：spawn_entity.py 的节点（如果未关闭）。
   ② 其他控制器相关节点。

解决方案

1. 检查 controller_manager 的启动状态
   运行以下命令，查看是否存在启动错误：

   ros2 run controller_manager spawner joint_state_broadcaster
   

   观察输出日志，看是否有报错信息。

2. 确认启动文件配置
   如果是通过 launch 文件启动仿真和控制器，检查 .launch.py 文件，确保包含以下内容：
   启动 controller_manager：

   Node(
   package="controller_manager",
   executable="ros2_control_node",
   parameters=[<path_to_controller_config>.yaml],
   )
   

   加载控制器：

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

3. 手动启动 controller_manager
   如果未通过启动文件启动，可以手动运行 controller_manager：

   ros2 run controller_manager ros2_control_node --ros-args --params-file <path_to_config>.yaml
   

   然后加载控制器：

   ros2 control load_controller --set-state active joint_state_broadcaster
   

4. 检查参数文件
   确认参数文件（通常是 YAML 格式）中正确定义了控制器。例如：

   joint_state_broadcaster:
   type: joint_state_broadcaster/JointStateBroadcaster
   publish_rate: 50
   

验证步骤

1. 再次运行以下命令，确认 controller_manager 是否正常启动：

   ros2 node list
   

2. 应该看到类似于 /controller_manager 的节点。
   检查服务是否可用：

   ros2 service list
   

   确认是否包含 /controller_manager/load_controller。

3. 加载并激活控制器，检查是否有报错。

   如果仍然有问题，请提供以下信息：
   使用的 ROS 2 和 Gazebo 版本。
   控制器的配置文件路径和内容。
   启动仿真和控制器的具体方法（如 launch 文件或手动命令）。

7.3 检查 controller_manager 的启动状态：ros2 run controller_manager spawner joint_state_broadcaster

xj@xj:~$ ros2 run controller_manager spawner joint_state_broadcaster
[WARN] [1735114476.343109473] [spawner_joint_state_broadcaster]: Controller already loaded, skipping load_controller
[ERROR] [1735114476.345189090] [spawner_joint_state_broadcaster]: Failed to configure controller
[ros2run]: Process exited with failure 1

从报错信息可以看到以下几点：

控制器已加载：

1. [WARN] Controller already loaded, skipping load_controller 表明 joint_state_broadcaster 控制器已经被加载，但未成功配置。
   配置失败：
2. [ERROR] Failed to configure controller 表明控制器在尝试配置时出错。这通常是由于参数配置问题或某些依赖（如硬件接口）未正确初始化导致的。

问题可能的原因

1. 参数文件配置错误：
   控制器的参数文件中缺少某些关键配置，或配置不兼容。
   joint_state_broadcaster 的必要参数（如 type）可能未正确设置。
2. Gazebo 与 ROS 2 界面问题：
   如果 Gazebo 的模型未正确加载（如 STL 文件路径问题），可能导致硬件接口未能初始化。
3. Gazebo 物理接口未正确绑定：
   控制器依赖的资源（如关节）可能未被正确声明或连接。
4. 控制器未被激活：
   即使控制器被加载，未能激活时也无法正常工作。

解决方案

1. 检查控制器的参数文件
   确保控制器的参数文件（通常是 YAML 格式）包含以下内容，并参数正确：

   controller_manager:
   	ros__parameters:
   		update_rate: 50 # 可根据需求调整更新频率
   
   joint_state_broadcaster:
   	ros__parameters:
   		type: joint_state_broadcaster/JointStateBroadcaster
   

   运行命令确认使用的参数文件路径是否正确：

   ros2 param list /controller_manager
   

2. 手动激活控制器
   尝试通过命令手动激活控制器：

   ros2 control set_controller_state joint_state_broadcaster active
   

   检查是否有报错信息。如果激活失败，则说明控制器依赖的资源未正确初始化。

3. 检查 Gazebo 加载模型的问题
   从之前的 Gazebo 日志中可以看到：

   URI not supported by Fuel [model://ok3/meshes/Link_1.STL]
   File or path does not exist [""] [model://ok3/meshes/Link_1.STL]
   No mesh specified
   

   这是 Gazebo 无法加载模型 ok3 的 STL 文件的警告和错误。这可能会导致 Gazebo 的物理接口无法正常工作，进而影响控制器的初始化。
   解决方法：
   ① 确认 ok3 模型的路径是否正确，以及其配置文件（如 model.sdf 或 model.config）中对 STL 文件的引用是否有效。
   ② 如果 STL 文件缺失或路径错误，手动修复模型路径或重新下载模型。

4. 重新加载和激活控制器
   在解决上述问题后，尝试重新加载并激活控制器：

   ros2 control unload_controller joint_state_broadcaster
   ros2 control load_controller joint_state_broadcaster
   ros2 control set_controller_state joint_state_broadcaster active
   

5. 检查其他依赖的控制器
   有些控制器（如 joint_trajectory_controller）可能依赖 joint_state_broadcaster。确保依赖链中的所有控制器均被正确加载和激活。

验证步骤

1. 再次检查运行中的节点和服务：

   ros2 node list
   ros2 service list
   

2. 确认 controller_manager 和控制器相关服务是否正常。
3. 观察 Gazebo 是否正确加载模型以及仿真是否正常。
4. 检查是否能够读取关节状态：

   ros2 topic echo /joint_states
   

   如果能看到关节数据输出，则说明 joint_state_broadcaster 已正常工作。
   如果问题仍未解决，请提供：
   使用的 YAML 参数文件内容。
   Gazebo 模型文件的具体路径和内容。
   更详细的报错日志。

7.4 确认使用的参数文件路径是否正确：ros2 param list /controller_manager

xj@xj:~$ ros2 param list /controller_manager
  joint_state_broadcaster.params_file
  joint_state_broadcaster.type
  manipulator_controller.params_file
  manipulator_controller.type
  qos_overrides./clock.subscription.depth
  qos_overrides./clock.subscription.durability
  qos_overrides./clock.subscription.history
  qos_overrides./clock.subscription.reliability
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  robot_description
  update_rate
  use_sim_time

从提供的 ros2 param list /controller_manager 输出来看，controller_manager 确实加载了与控制器相关的参数，包括 joint_state_broadcaster 和 manipulator_controller 的配置。以下是对这些参数的分析和下一步排查建议：
关键参数分析

1. joint_state_broadcaster.params_file 和 joint_state_broadcaster.type
   这两个参数是 joint_state_broadcaster 的核心配置。
   params_file 应该指定一个 YAML 文件路径，该文件定义了 joint_state_broadcaster 的具体参数设置。
   type 应为 joint_state_broadcaster/JointStateBroadcaster。
2. manipulator_controller.params_file 和 manipulator_controller.type
   类似地，manipulator_controller 的配置需要正确的参数文件路径以及控制器类型（如 joint_trajectory_controller/JointTrajectoryController）。
3. 其他参数
   robot_description 通常包含 URDF 文件内容（作为字符串），用于描述机器人模型。
   update_rate 是控制器管理器的更新频率，建议确认是否合理（如 50 或 100）。

检查配置

1. 检查 params_file 配置是否正确
   运行以下命令获取 joint_state_broadcaster.params_file 的具体值：

   ros2 param get /controller_manager joint_state_broadcaster.params_file
   

   确认路径是否有效，并检查对应的 YAML 文件内容。例如，文件内容应包含类似以下内容：

   joint_state_broadcaster:
   	ros__parameters:
   		type: joint_state_broadcaster/JointStateBroadcaster
   

   如果路径无效或文件内容错误，请修正后重新加载控制器。
2. 检查 type 是否正确
   运行以下命令确认 type 值：

   ros2 param get /controller_manager joint_state_broadcaster.type
   

   正常情况下应返回：

   joint_state_broadcaster/JointStateBroadcaster
   

   如果返回其他值，可能是配置错误，需要修正为正确的类型。
3. 验证 robot_description
   检查机器人模型是否被正确加载：

   ros2 param get /controller_manager robot_description
   

   • 输出应该是一段长字符串（URDF 文件内容）。如果为空或内容错误，可能导致控制器无法正确初始化。
   • 如果内容错误，需检查加载 URDF 文件的步骤，常用的方式是通过 robot_state_publisher 加载，例如：

   ros2 launch robot_state_publisher robot_state_publisher.launch.py model:=/path/to/your_robot.urdf
   

下一步操作

1. 检查控制器加载和激活
   在确认参数配置正确后，尝试重新加载和激活控制器：

   ros2 control unload_controller joint_state_broadcaster
   ros2 control load_controller joint_state_broadcaster
   ros2 control set_controller_state joint_state_broadcaster active
   

2. 观察 Gazebo 的日志输出
   确保 Gazebo 能正确加载模型，并且关节信息能被正常传播。如果仍出现以下错误：

   URI not supported by Fuel [model://...]
   File or path does not exist
   

   需要确认 Gazebo 模型的路径和配置文件是否正确。
3. 检查话题输出
   确认 joint_state_broadcaster 是否正常工作：

   ros2 topic echo /joint_states
   

   如果能看到关节状态的实时输出，则说明 joint_state_broadcaster 正常运行。

可能的改进建议

• 确保参数一致性：检查 params_file 和 type 的值是否正确且与 YAML 文件一致。
• 检查依赖顺序：在加载 joint_state_broadcaster 之前，确保机器人模型和物理引擎（Gazebo）均已正确启动。
• 进一步调试：如果问题持续，可以增加日志级别获取更多信息：

  ros2 run controller_manager spawner joint_state_broadcaster --ros-args --log-level debug
  

7.5 检查 params_file 配置的具体值：ros2 param get /controller_manager joint_state_broadcaster.params_file

控制台返回：

xj@xj:~$ ros2 param get /controller_manager joint_state_broadcaster.params_file
Parameter not set.
xj@xj:~$ ros2 param get /controller_manager joint_state_broadcaster.type
String value is: joint_state_broadcaster/JointStateBroadcaster

joint_state_broadcaster.params_file 参数未设置，这表明 joint_state_broadcaster 的配置可能有问题。没有正确的参数文件可能导致控制器无法正常加载或配置失败。

以下是解决问题的步骤：

1. 检查 params_file 参数是否必需
   在某些情况下，params_file 可能不是必需的。如果 joint_state_broadcaster 的配置只需要 type 参数，确保该参数已正确设置：

   ros2 param get /controller_manager joint_state_broadcaster.type
   

   正常情况下，输出应为：

   joint_state_broadcaster/JointStateBroadcaster
   

   如果 type 参数未设置或设置错误，请参考后续步骤。
2. 手动设置参数文件
   如果需要通过参数文件提供 joint_state_broadcaster 的配置，则需要设置 params_file 参数。以下是创建参数文件并配置的步骤：
   (1) 创建 YAML 参数文件
   在工作空间目录下创建一个文件，例如 joint_state_broadcaster.yaml，内容如下：

   joint_state_broadcaster:
   	ros__parameters:
   		type: joint_state_broadcaster/JointStateBroadcaster
   

   (2) 设置 params_file
   将文件路径传递给 controller_manager：

   ros2 param set /controller_manager joint_state_broadcaster.params_file /path/to/joint_state_broadcaster.yaml
   

   替换 /path/to/joint_state_broadcaster.yaml 为你实际的文件路径。
3. 加载并激活控制器
   在设置好参数后，重新尝试加载和激活控制器：

   ros2 control unload_controller joint_state_broadcaster
   ros2 control load_controller joint_state_broadcaster
   ros2 control set_controller_state joint_state_broadcaster active
   

4. 验证控制器是否正常工作
   确保控制器已正确激活，可以运行以下命令查看当前控制器的状态：

   ros2 control list_controllers
   

   如果控制器激活成功，输出应类似于：

   joint_state_broadcaster[active]
   

   此外，检查是否能通过话题 /joint_states 看到关节状态：

   ros2 topic echo /joint_states
   

5. 排查其他问题。如果问题依然存在，请检查以下方面：
   ① URDF 文件正确性：确保机器人模型正确加载，并包含关节的 joint 定义。
   ② Gazebo 配置：确认 Gazebo 中的模型能正确加载，关节可以被物理引擎管理。
   ③ 依赖顺序：确保在加载控制器前，robot_state_publisher 已正常工作，且 Gazebo 模型加载完成。
   ④ 如果按照以上步骤仍未解决问题，请分享更多日志信息或其他相关配置，我将进一步协助。

7.6 手动设置参数文件：ros2 param set /controller_manager joint_state_broadcaster.params_file ~/ws_ok/joint_state_broadcaster.yaml

xj@xj:~$ cd ws_ok
xj@xj:~/ws_ok$ touch joint_state_broadcaster.yaml
xj@xj:~/ws_ok$ ros2 param set /controller_manager joint_state_broadcaster.params_file ~/ws_ok/joint_state_broadcaster.yaml
Set parameter successful
xj@xj:~/ws_ok$ ros2 param get /controller_manager joint_state_broadcaster.params_file
String value is: /home/xj/ws_ok/joint_state_broadcaster.yaml

按照7.5的解决方法2. 手动设置参数文件操作，
然后在终端输入

ros2 param set /controller_manager joint_state_broadcaster.params_file ~/ws_ok/joint_state_broadcaster.yaml

gazebo终于打开了！而且gazebo的终端显示内容如下：
但是打开的gazebo是空白的;

此时，我的~/ws_ok/src的文件目录是这样的：

xj@xj:~/ws_ok/src$ tree
.
├── ok3
│   ├── CMakeLists.txt
│   ├── launch
│   │   └── display.launch.py
│   ├── meshes
│   │   ├── base_link.STL
│   │   ├── Link_1.STL
│   │   ├── Link_2.STL
│   │   ├── Link_3.STL
│   │   ├── Link_4.STL
│   │   ├── Link_5.STL
│   │   └── Link_6.STL
│   ├── package.xml
│   ├── rviz
│   │   └── view_robot.rviz
│   └── urdf
│       ├── ok3.urdf
│       ├── ok3.urdf.xacro
│       └── ok5-correct.urdf
└── ok3_moveit_config
    ├── CMakeLists.txt
    ├── config
    │   ├── initial_positions.yaml
    │   ├── joint_limits.yaml
    │   ├── kinematics.yaml
    │   ├── moveit_controllers.yaml
    │   ├── moveit.rviz
    │   ├── ok3.ros2_control.xacro
    │   ├── ok3.srdf
    │   ├── ok3.urdf.xacro
    │   ├── pilz_cartesian_limits.yaml
    │   └── 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
    └── package.xml

9 directories, 35 files

7.7 把urdf的路径改成绝对路径就可以了

原来

<mesh filename="package://ok3/meshes/base_link.STL" />

改后

<mesh filename="/home/xj/ws_ok/src/ok3/meshes/base_link.STL" />

打开后的gazebo终于不是空白了，我自己的机器人出现了，可是为什么一直抖呢？？

7.8 RViz场景中没有机器人

Gazebo+RViz联调：
分别打开两个控制台，然后分别执行以下命令

ros2 launch ok3_moveit_config gazebo.launch.py
ros2 launch ok3_moveit_config my_moveit_rviz.launch.py 

天塌了！我的RViz场景中没有机器人 @.@

终端打印：

[move_group-1] /home/xj/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group: error while loading shared libraries: libgeometric_shapes.so.2.1.3: cannot open shared object file: No such file or directory
[ERROR] [move_group-1]: process has died [pid 11855, exit code 127, cmd '/home/xj/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_n30izwta --params-file /tmp/launch_params_g6n0u_eo --params-file /tmp/launch_params_am4tao3s'].
[rviz2-2] [INFO] [1735192939.829918192] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1735192939.829976643] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1735192939.966097806] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [ERROR] [1735192940.086170103] [rviz2]: PluginlibFactory: The plugin for class 'moveit_rviz_plugin/MotionPlanning' failed to load. Error: Failed to load library /home/xj/ws_moveit/install/moveit_ros_visualization/lib/libmoveit_motion_planning_rviz_plugin.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library dlopen error: libgeometric_shapes.so.2.1.3: cannot open shared object file: No such file or directory, at ./src/shared_library.c:99

从结果可以看到系统中已经存在 libgeometric_shapes.so，并且版本是 2.3.1。然而，之前的错误提示表明程序需要的是 libgeometric_shapes.so.2.1.3，这表示依赖的版本不匹配。

以下是解决方法：

方法 1: 创建符号链接:
如果程序可以兼容较新的 libgeometric_shapes.so 版本，可以尝试创建一个指向现有库的符号链接：
切换到库文件所在目录：

cd /opt/ros/humble/lib/

创建符号链接：

sudo ln -s libgeometric_shapes.so.2.3.1 libgeometric_shapes.so.2.1.3

验证链接是否创建成功：

ls -l | grep libgeometric_shapes

再次尝试运行程序，看看问题是否解决。

然后就能打开RViz了，但是RViz里面没有显示机器人
终端报错：

[move_group-1] [INFO] [1735193564.711218439] [move_group.moveit.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-1] Error:   Error retrieving file [/home/xj/ws_ok/src/ok3/meshes/base_link.STL]: 
[move_group-1]          at line 277 in ./src/mesh_operations.cpp
[move_group-1] [WARN] [1735193564.711380767] [move_group.moveit.moveit.core.robot_model]: Link base_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[move_group-1] Error:   Error retrieving file [/home/xj/ws_ok/src/ok3/meshes/Link_1.STL]: 
[move_group-1]          at line 277 in ./src/mesh_operations.cpp
[move_group-1] [WARN] [1735193564.711810140] [move_group.moveit.moveit.core.robot_model]: Link Link_1 has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.

全部换成圆柱是可以联调的，应该是STL文件出了问题：