Gazebo與ros_control（4）：舉一反三，實戰youBot
在前面的三篇中，對ros_control也有個大致瞭解了。這篇就是將之前學到的用於我們實驗室的平臺KUKA youBot上。在此之前，其實網上已經有關於youBot在Gazebo下的模擬視訊了。
另外說個題外話，用Gazebo和Rviz都可以做模擬，Gazebo最強大的是擁有物理引擎，有物理渲染，有碰撞效果、重力等等。
這裡參考的package主要是：
建議大家直接照著第二個來，很快就可以用起來，它主要是啟動gazebo模擬環境，載入模型。也即上面文章的第二、三個階段，載入完模擬的控制器後就可以對它進行控制了。它的節點圖如下所示，已經暴露出來了控制手臂和夾子的action介面，同時還接受cmd_vel這個topic，釋出odom。
1. Topic直接控制
這個action介面與實際機器人的是一樣的，直接拿一個以前action教程的程式，跑一跑。發現Gazebo中的機器人也動起來了。
2. Moveit軌跡規劃輸出到Gazebo中的youBot上
同上一篇文章一樣，我們也可以利用Moveit來生成控制軌跡輸出控制youBot。視訊如下：
這裡主要的問題有兩個：
- navigation的時候，rviz中的模型可以動，但是Gazebo中的卻沒有運動。
- 我的fixed 座標用的是odom，相對於它，座標在用nav的時候場景也跟著旋轉了（機器人相對於場景並沒有變化），而實際上在ros_control（2）中，那個差速驅動的小車並不是這樣，這裡的運動很有問題。
這是我們系統上的所有的controller，可以看到裡面有我們下文將會提到的SteeredWheelBaseController，在倒數第二個。
我們利用上面的package載入的controller可以用命令檢視，如下：
3. 改動官方的youbot description自己新增控制介面
要學到東西，最好的辦法就是自己做一遍，確實我遇到了很多問題，但是發現慢慢思路也明朗不少。
最開始，我的youbot description是官方的，後來同學介紹，發現一個模型還不錯的包，最主要的區別就是輪子不再是四個球了，更真實一些。也即上面的第一個連結裡的description。
第二個我做了修改的地方是，夾子之間加了一個座標系，同時加入了xtion深度相機。
1. 第一步還是從urdf.xacro開始
這裡涉及檔案較多，請掏出A4紙，自己對著檔案一個個做好筆記。
youbot官方其實已經將xacro中包含了一些gazebo的transmission以及interface，也即lib*.so檔案。特別要注意的是urdf/youbot_base/base.gazebo.xacro這個檔案，這個檔案包含的是libgazebo_ros_planar_move.so它的作用就是將輸入的cmd_vel（底座的線速度和角速度）轉化為odom topic（包含機器人自己的位置、姿態以及線速度和角速度）。後來在搜尋的過程中，發現另一種寫法SteeredWheelBaseController。這裡埋下伏筆，主要是在odom，底座移動的時候才會做修改，如果只是控制手臂，先不用改。
有了這個之後第二步就是將模型載入到Gazebo。
2. 將模型載入到Gazebo
載入的時候是起名為youbot_world.launch
<launch>
<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"/>
<arg name="init_pos_x" default="0.0"/>
<arg name="init_pos_y" default="0.0"/>
<arg name="init_pos_z" default="0.1"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find youbot_gazebo)/worlds/laser_box.world"/>-->
<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>
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find youbot_description)/robots/youbot.urdf.xacro'" />
<!-- 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 youbot
-param robot_description
-x $(arg init_pos_x)
-y $(arg init_pos_y)
-z $(arg init_pos_z)
"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" respawn="false">
<param name="publish_frequency" type="double" value="50.0" />
</node>
</launch>3. 配置控制介面
我的檔案總的如下，是將它依據不同的controller分散到三個不同的檔案中，因為為了與實際的topic對上，arm跟gripper是有arm_1這個名稱空間的，所以它們兩個放在一個檔案裡，然後base_controller和joint_state_controller單獨作為一個。
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
arm_controller:
type: effort_controllers/JointTrajectoryController
joints:
- arm_joint_1
- arm_joint_2
- arm_joint_3
- arm_joint_4
- arm_joint_5
gains:
arm_joint_1: {p: 1000.0, i: 0.01, d: 0.1}
arm_joint_2: {p: 1000.0, i: 0.01, d: 0.1}
arm_joint_3: {p: 1000.0, i: 0.01, d: 0.1}
arm_joint_4: {p: 1000.0, i: 0.01, d: 0.1}
arm_joint_5: {p: 1000.0, i: 0.01, d: 0.1}
gripper_controller:
type: effort_controllers/JointTrajectoryController
joints:
- gripper_finger_joint_l
- gripper_finger_joint_r
gains:
gripper_finger_joint_l: {p: 1000.0, d: 1.0}
gripper_finger_joint_r: {p: 1000.0, d: 1.0}
base_controller:
pid_gains:
wheel_joint_fl: {p: 100.0, i: 0.0, d: 1.0}
caster_joint_fl: {p: 100.0, i: 0.0, d: 1.0}
wheel_joint_fr: {p: 100.0, i: 0.0, d: 1.0}
caster_joint_fr: {p: 100.0, i: 0.0, d: 1.0}
wheel_joint_bl: {p: 100.0, i: 0.0, d: 1.0}
caster_joint_bl: {p: 100.0, i: 0.0, d: 1.0}
wheel_joint_br: {p: 100.0, i: 0.0, d: 1.0}
caster_joint_br: {p: 100.0, i: 0.0, d: 1.0}
type: steered_wheel_base_controller/SteeredWheelBaseController
base_frame: base_footprint
linear_speed_limit: 1.5
linear_acceleration_limit: 2.5
linear_deceleration_limit: 2.5
yaw_speed_limit: 3.0
yaw_acceleration_limit: 3.2
yaw_deceleration_limit: 3.2
cmd_vel_timeout: 0.0 # disabled, as it is on the real platform
wheels:
- steering_joint: caster_joint_fl
axle_joint: wheel_joint_fl
diameter: 0.095
- steering_joint: caster_joint_fr
axle_joint: wheel_joint_fr
diameter: 0.095
- steering_joint: caster_joint_bl
axle_joint: wheel_joint_bl
diameter: 0.095
- steering_joint: caster_joint_br
axle_joint: wheel_joint_br
diameter: 0.095controller.yaml寫完之後，照例還是寫launch檔案載入它，因為有不同的yaml和namespace，而配置檔案只有一個名字，一次寫三個肯定有衝突，所以，首先在三個不同的launch檔案中呼叫controller_spawner，然後再在一個launch中包含它們三個。
arm_controller.launch它包含了namespace：arm_1
<?xml version="1.0"?>
<launch>
<arg name="arm_name" default="arm_1"/>
<!-- upload arm and gripper controller parameters -->
<rosparam file="$(find youbot_control)/config/only/arm_controller.yaml" command="load" ns="$(arg arm_name)"/>
<!-- spawn arm controller -->
<node pkg="controller_manager" type="spawner" name="arm_1_arm_controller_spawner"
args="$(arg arm_name)/arm_controller" respawn="false" output="screen" />
<!-- spawn gripper controller -->
<node pkg="controller_manager" type="spawner" name="arm_1_gripper_controller_spawner"
args="$(arg arm_name)/gripper_controller" respawn="false" output="screen" />
</launch>包含三個launch的檔案youbot_all_inone.launch
<?xml version="1.0"?>
<launch>
<include file="$(find youbot_control)/launch/joint_state_controller.launch" />
<include file="$(find youbot_control)/launch/base_controller.launch" />
<include file="$(find youbot_control)/launch/arm_controller.launch" />
</launch>4. 配置moveit
這裡我在配置7自由度機械臂的時候提過，它們都有一個共同的夾子間的座標系。
因此這裡就沒有參考youbot_integration中的moveit的配置，我的主要配置如下
groups
end effector
生成好了以後，照例配置controllers.yaml以及controller_manager.launch.xml
5.實驗
視訊：
節點關係圖：
問題：
因為youBot的底座是全向輪，預設的是libgazebo_ros_planar_move.so。關於它這裡也有個討論https://github.com/micpalmia/youbot_ros_tools/issues/5根據youtube上面的一個視訊，這個使用起來轉彎的時候會飄ROS Planar Move vs Gazebo on the youBot: turn https://www.youtube.com/watch?v=oHlSGvBii2Q。修改的這個是用的SteeredWheelBaseController，和youbot_simulation的一樣，最開始的問題是找不到，後來解決是因為發現base_gazebo.xacro做過非常大的修改，參考mars group的youbot_simulation解決掉了。然後的問題是如下圖這樣：發現它的base_controller並沒有載入起來，只加載了下圖最下面的三個，也就是目前只能控制手臂
附加資料：
=============================================================
youbot底座移動導航模擬：點選開啟連結
模擬鐳射雷達建圖：點選開啟連結