在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手

目前的空余时间主要都在研究ROS2，最终目的是控制自己用舵机组装的机械手。
由于种种原因，先控制Gazebo的自定义机械手。
先看看目前的成果
左侧是rviz2中的moveit组件的机械手，右侧是gazebo中的机械手。在moveit中进行路径规划并执行后，右侧gazebo中的机械手也就执行相应的动作。

1.软件的安装

1.1.Ubuntu 22.04

据说win10下也可以装ROS2，但是十分折腾，还是在Ubuntu下安装方便一些，Ubuntu22.04才有humble版本的ros2，Ubuntu20.04没有humble。

1.2.ROS2

按照鱼香ros的教程，在命令行中执行鱼香ROS的一键安装代码

wget http://fishros.com/install -O fishros && . fishros 

然后选择安装ros，选择humble版本。

1.3.Gazebo

参考教程
只要执行两行就行

sudo apt install gazebo11
sudo apt install ros-humble-gazebo-*

1.4.MoveIt2

参考教程
MoveIt2可以选择自己编译源码安装，或者直接从二进制安装。
个人建议直接二进制安装，可以省很多事。

sudo apt install ros-humble-moveit

1.5.moveit-setup-assistant

这是一个配套moveit的配置助手，有了它就可以方便地进行很多初始化的工作。

sudo apt install ros-humble-moveit-setup-assistant

2.机械手urdf文件的编写

我们用urdf文件来描述我们的机械手的外观以及物理性能。这里为了简便，就只用了基本的圆柱、立方体了。追求美观的朋友，还可以用dae文件来描述机械手的外形。
文件six_arm.urdf

<?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><!-- 在有需要时注释这个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>/home/yong/Desktop/myRobot/src/mybot/config/ros2_controllers.yaml</parameters><robot_param_node>robot_state_publisher</robot_param_node></plugin></gazebo></robot>

2.1.机械手的外型

可以在vscode中打开这个文件然后用插件就可以看到这个机械手的外形，关于urdf语法的详细介绍，可以看这里。

2.2.机械手link的inertial的设置

我这里偷懒，随便设置的。真正设置的话，有公式，自己可以参考一下。
urdf里面的link必须要有旋转惯量矩阵‘intertial’的，否则在gazebo里面导入模型urdf时，会报下面的错。
[gazebo-1] [Err] [Model.cc:123] Error Code 23 Msg: FrameAttachedToGraph error, Non-LINK vertex with name [model] is disconnected; it should have 1 outgoing edge in MODEL attached_to graph.

2.3.ros2_control插件

该插件是在gazbo导入该模型文件时，创建与Ros2交互的接口。

上面的ros2_controllers.yaml文件是在下一步创建出来的，先不用管。
同时，ros2_control这个节点下的内容也是要和ros2_controllers.yaml对应的，也可以先不管。
这个节点在开始阶段先注释掉，否则会与通过moveit_setup_assistant创建的一个fake_systemde ros2_control冲突。

3.机械手与MoveIt的关联

通过前面的操作，我们拥有了一个描述机械手的文件 six_arm.urdf，接下来我们利用该文件创建一个可以利用MoveIt进行路径规划的“工程”。

3.1.建立一个文件夹myRobot，然后再在此文件夹中建立一个src文件夹

3.2.然后进入src文件夹路径，创建包

ros2 pkg create mybot_description --build-type ament_python

3.3.在 src/mybot_description文件夹下，创建urdf文件夹，然后把six_arm.urdf放进去，如下图所示。

3.4.返回myRobot目录，然后编译一下

colcon build

3.5.source一下

source的目的是为了把我们创建的 mybot_description项目暴露给控制台，然后让后续的moveit_setup_assistant可以找到对应的urdf文件

source install/setup.bash 

3.6.利用moveit_setup_assistant创建项目

参考教程，启动moveit_setup_assistant

 ros2 run moveit_setup_assistant moveit_setup_assistant

记得选择src里面的urdf，然后后面的就按照教程来干了。
注意：group name设置my_group
一些关键的设置看下列图

在最后一步保存时，在我们前面创建的src文件夹里面，创建一个mybot文件夹
然后在助手里选择该路径，点击generat package

3.7.编译试运行

此时，退出助手，并执行编译；
编译完成后，重新source一下。

colcon build
source install/setup.bash 

记得把前面的urdf文件里面的这个路径改成你自己的。
这段注释一下，否则会有冲突。到后面就可以取消注释，但是目前要注释掉。

每次修改完文件后，记得都要 colcon build一下。

试着执行

ros2 launch mybot demo.launch.py

会报错

这是因为在编译时没有把urdf文件拷贝到install目录中，因此我们需要修改一下 src/mybot_description/setup.py
修改三处地方，将其修改成类似下面这样
修改完之后就可以自动在编译时复制到install目录了，具体原理可以自己按照代码推测一下

from setuptools import setupfrom 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='[e-mail]',description='TODO: Package description',license='TODO: License declaration',tests_require=['pytest'],entry_points={'console_scripts': [],},
)

colcon build一下。
然后启动

ros2 launch mybot demo.launch.py

然后就可以愉快地玩耍了。

假如报其他错误请查看此处。

4.机械手与Gazebo的关联

4.1.在 src/mybot/launch 路径下，新建一个文件： 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 FindPackageSharefrom launch.event_handlers import OnProcessExitdef 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 serverstart_gazebo_cmd =  ExecuteProcess(cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],output='screen')# Launch the robotspawn_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')# 关节状态发布器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')close_evt1 =  RegisterEventHandler( event_handler=OnProcessExit(target_action=spawn_entity_cmd,on_exit=[load_joint_state_controller],))close_evt2 = RegisterEventHandler(event_handler=OnProcessExit(target_action=load_joint_state_controller,on_exit=[load_joint_trajectory_controller],))ld = LaunchDescription()ld.add_entity(close_evt1)ld.add_entity(close_evt2)ld.add_action(start_gazebo_cmd)ld.add_action(node_robot_state_publisher)ld.add_action(spawn_entity_cmd)return ld

4.2.将 src/mybot_description/urdf/six_arm.urdf 的内容修改一下

将这一段取消注释。

4.3.编译、运行

每次修改完文件后，记得都要 colcon build一下。
colcon build一下。
然后启动

ros2 launch mybot gazebo.launch.py 

然后就可以愉快地玩耍了。

4.4. action send_goal 测试

目前运行的这这个gazebo例子已经开放了一个action接口，通过这个接口，我们就可以直接发送一个路径让其执行。

我们可以手写一个轨迹，然后让其执行
新建一个my_send_goal.sh文件，然后通过 chmod +x 使其具有执行权限


文件内容如下：

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 

接下来可以看到gazebo中的机械手按照我们设定的轨迹进行运动了。
可以尝试改变一下my_send_goal.sh 的内容，然后推测一下各个参数的意义。

5.MoveIt与Gazebo的关联

moveit提供了路径规划接口，gazebo提供了执行接口，因此只需要将他们连起来就实现了路径规划仿真了。

5.1.原理分析

在前面通过moveit_setup_assistant创建的包中，之所以可以进行路径规划并执行，是因为他既启动了moveit的组件，也启动了一个FakeSystem节点，该节点代替了实际的机械手反馈moveit的路径规划（提供action给moveit连接）。

从下图可以看出，当我们执行demo时，有好几个节点启动了。

但是假如我们需要将FakeSystem节点替换成gazebo的节点，就可以通过只启动需要的几个节点，然后不启动FakeSystem就行。

5.2.launch文件编写

按照5.1的分析，我们可以在src/mybot/launch下新建一个文件my_moveit_rviz.launch.py，
文件内容为

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_moveit_rviz_launchfrom 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 ParameterValuedef generate_launch_description():moveit_config = MoveItConfigsBuilder("six_arm", package_name="mybot_description").to_moveit_configs()ld = LaunchDescription()# 启动move_groupmy_generate_move_group_launch(ld, moveit_config)# 启动rvizmy_generate_moveit_rviz_launch(ld, moveit_config)return lddef 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 informationld.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 internallyadditional_env={"DISPLAY": ":0"},)return lddef 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

在文件中，我们主要启动了两个节点：move_group、rviz。
这里面这还涉及到一个use_sim_time的问题，因此才这样重写。参考这里
然后colcon build一下。

5.2.执行测试

分别打开两个控制台，然后分别执行以下命令

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

注意是要先运行gazebo（ros2 launch mybot gazebo.launch.py），然后再运行moveit（ros2 launch mybot my_moveit_rviz.launch.py ）
在moveit里面规划路径、然后执行。
在gazebo就可以看到模型动了起来。

6.总结

目前只是通过大致的操作完成了moveit控制gazebo里面的机械手，是偏操作方面的，原理可以粗略地理解成action的通讯。至于更加详细以及深入的教程，就等各位大佬来完成了。