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 &nb