ROS和Gazebo进行机器人仿真（二）
一.在Gazebo中使用ROS控制器
在本节中，我们将讨论如何在Gazebo中让机器人的每个关节运动。
为了让关节动起来，我们需要分配一个ROS控制器，尤其是，我们需要为每个关节连上一个与transmission标签内指定的硬件接口兼容的控制器。
ROS控制器主要由一套反馈机构组成，可以接受某一设定点，并用执行机构的反馈控制输出。
ROS控制器使用硬件接口与硬件交互，硬件接口的主要功能是充当ROS控制器与真实或仿真硬件之间的中介，根据ROS控制器生成的数据来分配
资源控制它。
在本机器人，我们定义了位置控制器，速度控制器，力控制器等，这些ROS控制器是由名为ros_control的一组软件包提供的。
为了正确理解如何为机械臂配置ROS控制器，我们需要理解它的概念。我们将进一步讨论ros_control软件包，不同类型的ROS
控制器以及ROS控制器如何与Gazebo仿真交互。
1.认识ros_control软件包
ros_control软件包实现了机器人控制器、控制管理器、硬件接口、不同的传输接口和控制工具箱。
ros_control软件包由以下各独立的软件包组成：
*control_toolbox : 这个软件包包含通用模块(PID 和 Sine)，可供所有控制器使用。
*controller_interface : 这个软件包包含了控制器的接口(interface)基类。
*controller_manager : 这个软件包提供了加载(load)、卸载（unload)、启动（start) 和停止（stop)等控制器的基础架构。
*controller_manager_msgs : 这个软件包提供了控制管理器的消息和服务定义。
*hardware_interface : 这个软件包包含了硬件接口的基类。
*transmission_interface : 这个软件包包含了传动（transmission)接口的接口类（差速、四杆联动、关节状态、位置和速度）。
2.不同类型的ROS控制器和硬件接口
包含标准ROS控制器的ROS软件包列表：
*joint_position_controller : 这是关节位置控制器的简单实现。
*joint_state_controller : 这是一个发布关节状态的控制器。
*joint_effort_controller : 这是关节力（强度）控制器的实现。
以下是ROS中常用的一些硬件接口：
*Joint Command Interface : 将命令发送到硬件。
*Effort Joint Interface : 发送effort命令。
*Velocity Joint Interface : 发送velocity命令。
*Position Joint Interface : 发送position命令。
*Joint State Interface : 从执行器编码器检索关节状态。
3.ROS控制器如何与Gazebo交互
如图可以看到ROS控制器是如何与Gazebo进行交互的，显示了ROS控制器，机器人硬件接口、仿真器/真实硬件的连接：
ROS控制器与Gazebo的接口图
我们可以看到第三方工具navigation和MoveIt软件包。这些软件包可以为移动机器人控制器和机械臂控制器提供目标位置（即设定点 ）。这些控制器可以将位置、速度或驱动力发送到机器人的硬件接口上。
硬件接口将每个资源分配给控制器，并将值发送给每个资源，机器人控制器与机器人硬件接口之间的通信如图所示：
ROS控制器和硬件接口的示意图
硬件接口与实际硬件和仿真分离，来自硬件接口的值可以馈送到Gazebo进行仿真或馈送到实际硬件本身。
硬件接口是机器人及其抽象硬件的软件表示，硬件接口的资源是执行机构、关节和传感器。
有些资源是只读的，比如关节状态、IMU、力-扭矩传感器等，有些资源是可读可写的，比如位置、速度和关节驱动力。
4.将关节状态控制器和关节位置控制器连接到手臂
将机器人控制器连接到每个关节是一项简单的任务，第一项任务就是 为两个控制器编写配置文件。
关节状态控制器将发布手臂的关节状态，而且关节位置控制器可以接收每个关节的目标位置并可以让每个关节运动。
我们将在seven_dof_arm_gazebo/config文件夹下找到控制器的配置文件seven_dof_arm_gazebo_control.yaml
以下是配置文件的定义：
1 seven_dof_arm:
2 # Publish all joint states -----------------------------------
3 joint_state_controller:
4 type: joint_state_controller/JointStateController
5 publish_rate: 50
6
7 # Position Controllers ---------------------------------------
8 joint1_position_controller:
9 type: position_controllers/JointPositionController
10 joint: shoulder_pan_joint
11 pid: {p: 100.0, i: 0.01, d: 10.0}
12 joint2_position_controller:
13 type: position_controllers/JointPositionController
14 joint: shoulder_pitch_joint
15 pid: {p: 100.0, i: 0.01, d: 10.0}
16 joint3_position_controller:
17 type: position_controllers/JointPositionController
18 joint: elbow_roll_joint
19 pid: {p: 100.0, i: 0.01, d: 10.0}
20 joint4_position_controller:
21 type: position_controllers/JointPositionController
22 joint: elbow_pitch_joint
23 pid: {p: 100.0, i: 0.01, d: 10.0}
24 joint5_position_controller:
25 type: position_controllers/JointPositionController
26 joint: wrist_roll_joint
27 pid: {p: 100.0, i: 0.01, d: 10.0}
28 joint6_position_controller:
29 type: position_controllers/JointPositionController
30 joint: wrist_pitch_joint
31 pid: {p: 100.0, i: 0.01, d: 10.0}
32 joint7_position_controller:
33 type: position_controllers/JointPositionController
34 joint: gripper_roll_joint
35 pid: {p: 100.0, i: 0.01, d: 10.0}
我们可以看到所有的控制器都位于命名空间seven_dof_arm中，第一 行代表关节状态控制器，他将以50hz的频率发布机器人的关节状态。
其他的控制器是关节位置控制器，它被分配给前七个关节，而且还定义了PID增益。
5.在Gaazebo中启动ROS控制器
如果控制器配置准备就绪，我们就可以创建一个启动文件，该文件将启动所有控制器并进行Gazebo仿真.
进入seven_dof_arm_gazebo/launch文件夹并打开seven_dof_arm_gazebo_control.launch文件：
1 <launch> 2 <!-- Launch Gazebo --> 3 <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_world.launch" /> 4 5 6 <!-- Load joint controller configurations from YAML file to parameter server --> 7 <rosparam file="$(find seven_dof_arm_gazebo)/config/seven_dof_arm_gazebo_control. yaml" command="load"/> 8 9 10 <!-- load the controllers --> 11 <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn=" false" 12 output="screen" ns="/seven_dof_arm" args="joint_state_controller 13 joint1_position_controller 14 joint2_position_controller 15 joint3_position_controller 16 joint4_position_controller 17 joint5_position_controller 18 joint6_position_controller 19 joint7_position_controller"/> 20 21 22 <!-- convert joint states to TF transforms for rviz, etc --> 23 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_ publisher" 24 respawn="false" output="screen"> 25 <remap from="/joint_states" to="/seven_dof_arm/joint_states" /> 26 </node> 27 28 </launch>
该启动文件可以启动手臂的Gazebo仿真，加载控制器配置信息、关节状态控制器和关节位置控制器，最后运行机器人状态发布者（负责发布关节状态和tf)
检查运行此启动文件后生成的控制器话题：
$ roslaunch seven_dof_arm_gazebo seven_dof_arm_gazebo_control.launch
如果命令执行成功，我们可以在终端看到如图所示的消息：
运行该启动文件时从控制器中生成的话题如下：
6.控制机器人的关节运动
完成以上步骤后，我们就可以开始对每个关节进行控制了。
要在Gazebo中控制机器人关节运动，我们需要使用std_msgs/Float64类型的消息将所需的关节值发布到关节位置控制器命令话题上。
如下是控制第四个关节运动到1.0度的位置：
$ rostopic pub /seven_dof_arm/joint4_position_controller/command std_msgs/Float64 1.0
还可以用以下命令查看机器人的关节状态
$rostopic echo /seven_dof_arm/joint_states
