寫在前面

通過上兩節的部落格，相信朋友們已經對gazebo的一些標籤有了較為深入的認識，但是特別是上一節，程式寫完了之後發現並沒有什麼特別的變化。著實，到目前為止，我們只是很執拗的在rviz和gazebo之間進行顯示，但是由於兩個模組是完全不一樣的，內部的訊息機制以及驅動機制都是差的比較多的，所以要想把兩者嫁接起來，中間還必須有一個轉換這的角色，這就是這個部落格要講的ros_control和plugin。

整體流程框架

依舊祭出官方的框架圖，今天其實我們會逐漸的進入到這個框架圖的內部，感受這個框架的魅力

提前宣告一下，在整張圖裡面我們基本上不用關注右上角的“Reality”，因為這個框框裡面顯示的都是真實的硬體裝置，由於我們使用的是硬體模擬（也就是gazebo），所以關注點放在下面和左上就可以了。

把大象裝進冰箱裡的第三步——連線gazebo和ros

回顧上一節的內容，我們在urdf檔案中，對一些關節（joint）打上了給gazebo使用的transmission標籤，我個人的理解是：transmission把TF（ros）的連線關係與模擬平臺上的驅動裝置（gazebo）聯絡在了一起，這樣，當物理引擎下，這個驅動裝置進行了動作的時候，ros就能知道是哪個TF需要變化；但是同理，我們要明白的一點是：transmission只是將兩個標籤聯絡在了一起，但是並不代表ros傳送一個訊息，gazebo中的該驅動裝置就能收到！還是那句老話，ros中機器人形態的變化只需修改對應的TF就可以了，但是gezebo中必須要有物理層面的輸入。

那麼，為了將兩者狠狠的聯絡起來，勤勞的程式設計師們就創造了一箇中間者——ros_control和gazebo plugin

gazebo plugin

gazebo plugin就是在ros和gazebo之間的一座橋樑了，下面是官方的原話：

Gazebo plugins give your URDF models greater functionality and can tie in ROS messages and service calls for sensor output and motor input. In this tutorial we explain both how to setup preexisting plugins and how to create your own custom plugins that can work with ROS.

可見當你的urdf中有了plugin之後，你的gazebo就與ros之間有了通訊的可能，這時候，很多朋友估計想起了上一節中的一個plugin——gazebo_ros_control。其實這個plugin有些特殊，私認為這個plugin其實就是讓所有transmission標籤生效的一個外掛，我們可以看看官方的原話：

In addition to the transmission tags, a Gazebo plugin needs to be added to your URDF that actually parses the transmission tags and loads the appropriate hardware interfaces and controller manager. By default the gazebo_ros_control plugin is very simple, though it is also extensible via an additional plugin architecture to allow power users to create their own custom robot hardware interfaces between ros_control and Gazebo.

可以看到這個外掛的作用一方面是要解釋transmission標籤，另一方面，它要載入合適的hardware interface和controller manager。回到最開始的整體框架圖中，我們可以清晰的看到官方將這個plugin與其他的Custom plugins也做了區分，表明說gazebo_ros_control plugin是與其他plugin不同的，而這個plugin可以看到，它專門接管的就是具有transmission的joint，也就是執行器這個部分。因此，私以為這個plugin的作用其實就是控制關節中的執行器運動，同時將運動後的資訊傳遞出來。那麼這裡需要澄清的一點就是，這個plugin其實並沒有將gazebo和ros連線起來（筆者這裡所說的連線是指在ros中可以通過話題topic或者服務service來控制），其作用應該是提供了一個能控制gazebo中執行器運動的途徑。

以上只是想打消一下各位朋友的疑問，同時也是怕之後在講真正的plugin（圖中的Custom Plugins）的時候，大家心中會一直想著這個plugin。

————————手動分割一下——————————

以下內容就是針對圖中的Custom Plugins了，這些plugin就具有將ros和gazebo連線起來的能力，也就是使用了這樣的外掛之後，你就可以在ros中直接的訂閱相應的話題並控制你的機器人了～

讓機器人動起來

做了這麼長時間的機器人了，一直就是看，這次終於要動起來了，其實只要在urdf中加入能讓機器人動作的外掛就可以啦，例如官方推薦的這個driver

    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>50.0</updateRate>
            <leftJoint>car_base_front_left_wheel</leftJoint>
            <rightJoint>car_base_front_right_wheel</rightJoint>
            <wheelSeparation>1.0</wheelSeparation>
            <wheelDiameter>0.5</wheelDiameter>
            <torque>1.0</torque>
            <commandTopic>cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <robotBaseFrame>base_link</robotBaseFrame>
            <publishWheelTF>true</publishWheelTF>
            <publishWheelJointState>true</publishWheelJointState>
            <legecyMode>false</legecyMode>
            <wheelAcceleration>1</wheelAcceleration>
        </plugin>
    </gazebo>

這裡面leftJoint和rightJoint要填寫你模型中的名稱，然後下面的兩個wheel separation和wheel diameter也要根據你的輪子相距的距離和輪子的直徑，此時如果你再使用urdf_sim_tutorial包進行顯示的時候，在另一個終端下檢視ros的話題時，你就能發現多了一個cmd_vel的話題了

此時使用下面的指令，你就可以看到小車動起來了～特別提醒，點選view->Wireframe和view->Link Frames能看的更清晰哦：D

rostopic pub -1 /cmd_vel geometry_msgs/Twist -- '[1.0,0,0]' '[0,0,0.0]'

1. 這裡要提示一下，因為這個plugin是屬於Custom Plugins中的，因此使用的時候和上一節中的gazebo_ros_control那個plugin會衝突，因此在使用前請將gazebo_ros_control的程式碼註釋掉，當然這個plugin也不是沒有用的，它結合ros_control照樣能讓小車動起來，後面會介紹一下。
2. 如果此時朋友們在rviz中觀察小車的TF的時候，會發現只有兩個輪子的TF是不停更新的，另外兩個輪子是不動的，但是gazebo中的輪子確實是四個都在動，原因主要是因為你在這個plugin中只綁定了兩個輪子（確切的說是你將publishWheelTF和publishWheelJointState宣告為true了，如果是false的話，rviz中輪子的TF也是不會動的哦），gazebo中另外兩個輪子動是因為輪子與地面有摩擦力，而ros單純檢測TF，你不發他們的資訊，我死活就是不動。
3. 上面的rostopic指令每次只發一次就可以了，因為這個plugin接到之後會一直讓joint以一定速度旋轉，想讓小車停下的話就傳送全0的指令就可以啦

————————又手動分割一下——————————

讓機器感受到自己的位姿——IMU外掛

其實經過上面的這個例子，想必朋友們對plugin已經有一些瞭解了，想用它很簡單，就直接將它“插入”到某個link或者joint中就可以了，剛才我們將diff_drive插入到了joint中，這次我們就將外掛“插入”一個link中去。
1. 首先在urdf中建立一個imu的link

    <link name="imu_link">
        <visual>
            <xacro:box_geometry width="${imu_size}" length="${imu_size}" height="${imu_height}"/>
            <material name="yellow"/>
        </visual>
        <collision>
            <xacro:box_geometry width="${imu_size}" length="${imu_size}" height="${imu_height}"/>
        </collision>
        <xacro:default_inertial mass="0.05"/>
    </link>

這裡面的引數大家自己定義就好了，這裡不再贅述。
2. 隨後對這個link插入一個IMU的外掛

<gazebo reference="imu_link">
        <gravity>true</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>100</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
                <topicName>imu</topicName>
                <bodyName>imu_link</bodyName>
                <updateRateHZ>100.0</updateRateHZ>
                <gaussianNoise>0.0</gaussianNoise>
                <xyzOffset>0 0 0</xyzOffset>
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu_link</frameName>
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
    </gazebo>

gazebo標籤的reference屬性就是讓gazebo知道隨後的程式碼是依附於誰的，這個reference的內容可以是一個link，也可以是一個joint。特別強調的是，plugin標籤一定是要作為sensor標籤的一部分，在sensor標籤內部出現，否則是不起作用的！！
3. 此時使用urdf_sim_tutorial啟動模型，同時使用rostopic檢視的時候，就會在topic中發現先一個名為/imu的話題，當然你有100種方法去檢視這個值，這裡我使用gazebo中的window->Topic Visulization中的IMU訊息進行檢視，效果如下圖所示：

可以看到這個資訊給的是很全的了，不僅給除了角速度和加速度，同時連四元數都給你了，簡直不能再壕了，不過美中不足的就是這個IMU可謂是一點兒溫飄都沒有，yaw軸就不會像我們結算出來的那樣存在漂移。

tips：當然gazebo還有很多外掛提供給開發者，一般情況下我覺得這些外掛是完全夠用了，具體的外掛使用大家可以到這裡檢視

ros_control

說完gazebo的plugin機制了，我們再來講一下ros control這個東東，流程圖中其實也給這個模組了很大的份量，但是到現在，我們似乎是沒有理由在去折騰這個東西了，原因也很簡單，上面的兩個plugin基本上已經能覆蓋我們使用的情況了，機器人也能動了，官方也提供了很多感測器外掛了，我們的大象已經裝到冰箱裡面了呀。粗略的想一下，emmm，是這樣的，但是仔細想想，我現在只是能控制差分旋轉了，不是差分的呢？像舵機這樣的位置控制電機又要怎麼辦呢？

所以，我們的裝大象之旅還沒有結束，還需要把這個小尾巴給實實在在的裝進去。

————————又雙手動分割一下——————————

回想一下，我們在程式中使用transmission標籤把joint關節轉換成了各式各樣的在gazebo下的電機，隨後通過對於整個機器人插入了一個叫做gazebo_ros_control的外掛使的這些transmission生效，讓這些電機與關節對應了起來，但是電機是gazebo中的啊，它即不能傳送ros的訊息，也不能訂閱ros的訊息，因此對於ros而言，這些電機是毫無用處的，因為不可讀，也不可寫；同理，對於gazebo，不管因為什麼因素，我的電機轉起來了之後，我只管模擬，沒有必要每個週期再去計算一下TF變換了多少，整個機器人相對於原點移動了多少。

那麼ros_control的作用就呼之欲出了：擔任gazebo與ros的翻譯者（大神們如果覺得不妥的話，請一定指出），讓兩個世界彼此可以瞭解對方。特別需要說明的是，這個翻譯者僅僅對有transmission標籤的關節有效，如果沒有的話，那肯定是不行的。

回到框架圖中，除去上面的兩個大方框，我們可以看到，當ros的訊息傳遞過來的時候(綠色的方框)，它並沒有給gazebo，而是給了一個controller，經過controller中的控制器計算出來相應的值之後，資料流入了JointCommand介面(圖下面有一個eg：EffortJointInterface的字樣，說明該介面是有許多種的)中，然後由這個介面轉交給gazebo模組；當gazebo計算資料完成之後，資料也是先流入一個叫做JointState的介面中，隨後又流入了一個controller中，不過這個controller的目的就很明顯了，就是計算整個機器人的TF，之後把TF釋出出去。

其中不得不說一下最耀眼的Controller Manager，這裡先劇透一下說這些controller其實都是通過配置檔案的形式載入到ros的引數伺服器中（param service），之後通過ros的服務（service）來啟動它們，如果每次都這麼幹的話，勢必很浪費時間和精力，因此ros_control就建立一了Controller Manager的東西來幫助我們管理這些服務。

那下面就介紹一下如何使用ros_control讓機器人動起來

配置檔案

作用

主要就是把controller的配置引數給記錄下來，下面的檔案就是我工程中的配置檔案：

/:
  joint_state_controller:
    type: "joint_state_controller/JointStateController"
    publish_rate: 50

  mobile_base_controller:
    type: "diff_drive_controller/DiffDriveController"
    left_wheel  : [ 'front_left_wheel', 'end_left_wheel' ]
    right_wheel : [ 'front_right_wheel', 'end_right_wheel' ]
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

    base_frame_id: base_link

    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 0.2   # m/s
        has_acceleration_limits: true
        max_acceleration       : 0.6   # m/s^2
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 2.0   # rad/s
        has_acceleration_limits: true
        max_acceleration       : 6.0   # rad/s^2

需要注意的就是left_wheel和right_wheel中填寫的一定是wheel joint的名稱，不要填寫為link的名稱了。

最後我們再來把所有的命令寫為一個launch檔案，如下：

<launch>
    <arg name="model" default="$(find ros_gazebo_learn)/urdf/rbo.urdf.xacro"/>
    <arg name="rviz_config" default="$(find urdf_tutorial)/rviz/urdf.rviz"/>
    <arg name="use_gui" default="True"/>

    <!-- rosparam -->
    <param name="robot_description" command="$(find xacro)/xacro.py --inorder $(arg model)"/>
    <!-- joint state publisher -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

    <!-- gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
    </include>

    <!-- spawn model -->
    <node name="robot_gazebo_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -model rbo -param robot_description">
    </node>

    <!-- load controller yaml -->
    <rosparam command="load" file="$(find ros_gazebo_learn)/config/joints.yaml"/>
    <!-- controller manager -->
    <node name="controller_manager" pkg="controller_manager" type="spawner" ns="/"
        args="joint_state_controller mobile_base_controller"/>

    <!-- rviz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" />

</launch>

這裡面每個節點什麼含義這裡就不做解釋了，大家應該也都能看懂，執行起來之後在rostopic下就能看到這樣一個話題：

其中的/mobile_base_controller/cmd_vel就是驅動所要訂閱的話題，我們用rostopic的形式傳送命令，就可以看到小車動起來了：

rostopic pub -r 30 /mobile_base_controller/cmd_vel geometry_msgs/Twist -- '[1.0,0,0]' '[0,0,0.0]'

tips： 這裡我也是介紹的速度控制電機的例子，希望朋友們自己下去可以試一下位置控制電機或者功率控制電機，主要就是熟悉一下需要改什麼，怎麼改

總結

那其實到這裡，gazebo和ros之間就已經聯絡起來了，這時候你就可以用gazebo做不少事情啦～slam也好，路徑規劃也好，其實都可以在這上面試一下～