一、 ROS中的控制器插件
ros_control的功能：
- ROS为开发者提供的机器人控制中间件
- 包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等
- 可以帮助机器人应用功能包快速落地，提高开发效率
下面介绍一下采用ros_control实现的控制系统整体架构：
- 控制器管理器
提供一种通用的接口来管理不同的控制器，比如小车的轮子需要速度指令，机械臂需要位置、扭矩指令，不同的控制器就需要一个controller manager来管理，以同时包含多种controller - 控制器
读取硬件状态，发布控制命令，完成每个joint的控制 - 硬件资源
为上下两层提供硬件资源的接口 - 机器人硬件抽象
机器人硬件抽象和硬件资源直接打交道，通过write和read方法完成硬件操作 - 真实机器人
执行接收到的命令
controller中主要实现的是PID控制，完成速度、位置等闭环控制，并将指令发出去，发给谁呢，当然是机器人，如果是仿真就发给gazebo，如果是实际机器人，就发给真实机器人硬件。这里就有一个interface，通过串口，网口等接口的形式把数据发出去hardware_interface是为了让上层的接口屏蔽底层的硬件，作为一个硬件封装层封装了底层的电机、执行器。接收到上层的指令后，再分发到底层的硬件。比如发给小车车轮，上面通常会有一个嵌入式板卡来完成本地的闭环控制，比如发送电压信号给电机，电机如果配备编码器，还可以通过硬件抽象层反馈上来，发回controller，再形成闭环。
ros_control这个功能包包含了很多controllers，完成速度、位置、力控制。常用的有四个controller:
joint_state_controller
这个controller其实和控制没关系，比较容易让人误解，监控机器人状态，比如编码器的反馈回来的位置速度等数据，它将其封装成topic发出来，功能和joint_state_publisher这个节点是一样的，只是数据输入不一样，输出都是话题形式。joint_effort_controller
joint_position_controller
机械臂经常会用这个controllerjoint_velocity_controller
此处会介绍 joint_state_controller和joint_position_controller，最后会再介绍一个更加上层的controller。因此ros_control在整个ROS框架中的作用就是作为一个中间件，衔接上层应用和真实机器人或仿真模型。
2.完善机器人模型
要放到gazebo中仿真，之前的urdf模型文件还需要再进行完善。
2.1 模型文件
第一步：在可视化基础上为link添加惯性参数和碰撞属性
一个技巧是设置很小的质量和较大的惯性矩阵，这样导入gazebo后模型运行会比较稳定，当然前提是仿真应用对这些物理参数要求不高。
第二步：为joint添加传动装置
代表减速器等传动装置，传进来的是控制指令，比如下图的位置指令
第三步：添加gazebo控制器插件
电机如何转起来，需要添加一个控制器，对应真实机器人的伺服驱动器，就是前面讲的controller，完成电机的运动控制，输入位置，输出到transmission上面去。参数是比较多的，一般在模型中加载插件，再通过多个参数文件具体配置。
robotNamespace代表具体的机器人命名空间，仿真类型robotSimType，使用默认的硬件抽象层接口，完成上面的指令到机器人的转换。legacyModeNS用于兼容之前ROS版本的配置。
2.2 在gazebo中加载机器人
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 启动gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find probot_description)/urdf/probot_anno.xacro'" />
<!-- 在gazebo中加载模型-->
<!--Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model probot_anno -param robot_description"/>
</launch>
launch文件完成的功能：一是启动gazebo仿真环境，二是将模型加载到仿真环境中去。
i5@i5-ThinkPad-T470:~/catkin_ws$ roslaunch probot_gazebo probot_anno_gazebo_world.launch
现在的机械臂还是不能运动的，可以看一下话题：
i5@i5-ThinkPad-T470:~/catkin_ws$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/rosout
/rosout_agg
3. 构建MoveIt!+Gazebo仿真
首先介绍一下moveit机器人控制框架，moveit输入用户指令，比如起始位置，输出轨迹数据，每个点通过位置速度加速度描述，为了通过话题发出来，还需要一个Follow Joint Trajectory功能，通过通信接口发给机器人，机器人还需要对轨迹进行精插补，再驱动电机同步运动。机器人控制器还需要将每个电机的状态数据反馈给moveit来确定机器人是否到达指定位置，这样完成一个闭环。
主要是理解三个模块的作用：Follow Joint Trajectory，Joint Trajectory Controller, Joint State Controller
先看gazebo这一端的，轨迹通过Action机制发出来，因此gazebo这边是一个action server，moveit那边是一个action client，sever这一端接收轨迹，完成插补运算，再发到每一个电机位置控制接口上。
ROS提供的关节轨迹控制器插补运算（具体可以看ros_controllers的源码）
- 线性样条：位置连续，速度、加速度不连续
- 三次样条：位置和速度连续，加速度不连续
- 五次样条：位置、速度、加速度都连续（ROS默认使用的）
image.png
电机转起来涉及到两个配置文件，一个是config/probot_anno_trajectory_control.yaml，用于明确控制器接口接收哪几个关节信息，要与urdf模型对应，文件中还包括每个关节的PID参数
probot_anno:
arm_joint_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
gains:
joint_1: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_2: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_3: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_4: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_5: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_6: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
另一个文件是控制器启动文件：probot_anno_trajectory_controller.launch，首先将上面的文件加载到参数服务器，再调用spawner根据参数文件内容启动arm_joint_controller，这个控制器完成每个轴的插补运算，并让每个轴运动。
<launch>
<rosparam file="$(find probot_gazebo)/config/probot_anno_trajectory_control.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/probot_anno" args="arm_joint_controller"/>
</launch>
以上完成的就是框架中绿色部分的配置。
joint state controller配置是类似的，先进行参数配置：probot_anno_gazebo_joint_states.yaml
probot_anno:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
用于监控机器人实时状态，joint_state_controller/JointStateController就是ros cotrollers四种当中的一种，参数就是反馈的频率。配置好后，需要启动它probot_anno_gazebo_states.launch：
<launch>
<!-- 将关节控制器的配置参数加载到参数服务器中 -->
<rosparam file="$(find probot_gazebo)/config/probot_anno_gazebo_joint_states.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/probot_anno" args="joint_state_controller" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/probot_anno/joint_states" />
</node>
</launch>
先加载文件，再通过spawner启动控制器，后面还启动了robot_state_publisher，用于根据joint_states发布TF坐标系.
第三个配置是框图中左边的，Follow Joint Trajectory，也是配参数controllers_gazebo.yaml：
controller_manager_ns: controller_manager
controller_list:
- name: probot_anno/arm_joint_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
再启动probot_anno_moveit_controller_manager.launch.xml:
<launch>
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- gazebo Controller -->
<rosparam file="$(find probot_anno_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>
下面梳理一下所有文件，首先是最顶层的probot_anno_bringup_moveit.launch:
<launch>
<!-- Launch Gazebo -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_world.launch" />
<!-- ros_control arm launch file -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_states.launch" />
<!-- ros_control trajectory control dof arm launch file -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_trajectory_controller.launch" />
<!-- moveit launch file -->
<include file="$(find probot_anno_moveit_config)/launch/moveit_planning_execution.launch" />
</launch>
首先启动仿真环境，接着启动Joint state反馈控制器，接着启动完成插补功能的控制器，最后启动moveit，里面会自动包含刚才的moveit一端封装action数据接口的功能。
总结一下几个核心的文件功能：
probot_anno_bringup_moveit.launch:最顶层的，启动下面三个文件和moveit里的文件
probot_anno_gazebo_world.launch: 启动仿真环境
probot_anno_gazebo_states.launch: 启动joint state controller
probot_anno_trajectory_controller.launch：启动插补运算的controller
probot_anno_moveit_controller_manager.launch.xml: 启动follow joint trajectory功能
i5@i5-ThinkPad-T470:~$ roslaunch probot_gazebo probot_anno_bringup_moveit.launch
最后梳理一下启动仿真的各个文件包含关系：
