Gazebo構建小車模型并通過ROS控制

• 介紹
• 編寫車子的URDF文件
• 編寫控制小車移動的插件(與ROS交互)
• 結尾

介紹

?突然想試試Gazebo這款仿真軟件，因為它可以讓你在任何時候都有機器人玩。但Gazebo的機制也比較復雜，所以還是先學習一下如何搭一個簡單的小車，并通過ROS平臺完成對小車的控制。

編寫車子的URDF文件

?這里是跟著《ROS機器人開發》書中的介紹一步步來的，對詳細過程有興趣的朋友去看一下。為了方便，直接把代碼和注釋都寫在一起，但運行時要注意的是：URDF文件里不要有中文注釋：

<!--URDF文件的注釋方法，即XML語法的注釋,但不能使用中文注釋-->
<!--但是這個ｕｒｄｆ只能用于在rviz中展示機器人模型，要想在gazebo中顯示，需要將urdf換成ｓｄｆ格式。好在開發者提供了專門的轉換命令，用戶只需要在urdf文件中添加一些額外的標簽，如：gazebo與rviz對應的link顏色設置代碼不同，所以要對urdf中material塊加入gazebo標簽，這些標簽放在</robot>之前就行-->
<?xml version='1.0'?>
<!--這里給整個完整的機器人進行命名，整個機器人就相當于Ｃ＋＋中的類對象。機器人的零件link，零件間關系joint相當于類成員以及成員間關系-->
<robot name="dd_robot"><!--定義一個機器人對象，整個<robot>代碼塊對應一個機器人模型--><link name="base_link"><!--base_link是特殊的link，之后機器人所有包含的link都會基于這個base_link來定義--><visual><!--設置某個link的顯示效果(即在rviz等軟件中的顯示效果)，如它的坐標、幾何屬性(用什么幾何形狀表示)--><!--這里沒聲明visual　name則默認base_link--><origin xyz="0.0 0.0 0.0"  rpy="0.0 0.0 0.0"/><!--link的方向和坐標(link中心的坐標)屬性(origin)--><geometry><!--link的幾何屬性-->　<box size="0.5 0.5 0.25" /><!--用一個box來表示該link，設置box的長寬高--></geometry><!--設置這個link的顏色，如果下一個visual沒有設置其他顏色，則整個link都使用這個顏色--><!--一旦制定了某個名稱的material，則這個名稱的material可以復用，即在其他visual塊中調用<material name="xxx"/>即可--><material name="blue"><color rgba="0.0 0.5 1.0 1.0"/></material></visual><!--為每個可視化元件添加碰撞屬性，以便讓gazebo知道各個元件的具體邊界位置要將visual和collision屬性分開寫，它們一個是視覺屬性，一個是碰撞屬性--><collision><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/><geometry><box size="0.5 0.5 0.25"/></geometry></collision><!--一般collision和visual塊中的幾何屬性是一樣的，因為一個物體外表要和它的形體相一致--><!--inertial塊代表link的慣性、運動學特性，它是模型在gazebo中能夠顯示的重要元素。每有一個geometry就需要有一個inertial--><inertial><mass value="5"/><!--mass的值是某個link的重量，單位時千克--><inertia ixx="0.13" ixy="0.0" ixz="0.0" iyy="0.21" iyz="0.0" izz="0.13"/><!--inertia為3x3的轉動慣量矩陣框架，inertia要根據link實際使用的geometry內容而定，可以去網站上找到相關計算公式--></inertial><!--一個link塊中可以添加多個visual塊，相當于這個link由代碼塊中所有visual組成。同個link中的所有visual是個整體，它們的運動是一致的--><visual name="caster"><origin xyz="0.2 0.0 -0.125" rpy="0.0 0.0 0.0"/><geometry><sphere radius="0.05"/></geometry></visual><collision><origin xyz="0.2 0.0 -0.125" rpy="0.0 0.0 0.0"/><geometry><sphere radius="0.05"/></geometry></collision></link><!--右邊輪子的link,編寫思路和上面的一樣--><link name="right_wheel"><visual><!--輪子的中心坐標可以設置為0,0，0，最終輪子的位置是會根據使用的joint確定--><origin xyz="0.0 -0.0 0.0" rpy="1.570795 0.0 0.0"/><geometry><cylinder length="0.1" radius="0.2"/></geometry><!--設置link的顏色--><material name="black"><color rgba="0.05 0.05 0.05 1.0"/></material></visual><collision><origin xyz="0.0 0.0 0.0" rpy="1.570795 0.0 0.0"/><geometry><cylinder length="0.1" radius="0.2"/></geometry></collision></link><!--連接右邊輪子和底座兩個links的關節joint--><joint name="join_right_wheel" type="continuous"><!--設置joint名稱和類型。這里使用連續類型的關節--><!--設置關節所連接的對象，一般一個關節連接兩個對象：父子兩個對象。對象名稱就是之前定義的那些link的名稱--><parent link="base_link"/><!--設置父對象--><child link="right_wheel"/><!--設置子對象--><!--下面設定的joint的中心是子對象與父對象間的相對位移。之前設定的子對象link的中心坐標也是在這個joint對應的相對位移的基礎上確定好的--><origin xyz="0.0 -0.30 0.0" rpy="0.0 0.0 0.0"/><!--設置joint的方向之類的位置信息--><axis xyz="0.0 1.0 0.0"/><!--因為連續型關節能圍繞一個軸旋轉，所以這里要設置這個關節能繞哪個軸旋轉--></joint><!--設置左邊輪子，和右邊輪子的思路一樣--><link name="left_wheel"><!--左輪的一些物理屬性可以和右邊輪子一樣--><visual><origin xyz="0.0 0.0 0.0" rpy="1.570795 0.0 0.0"/><geometry><cylinder length="0.1" radius="0.2"/></geometry><!--設置這個visual的顏色。由于之前定義了一個名為black的material，所以這里可以直接調用這個顏色--><material name="black"/></visual><collision><origin xyz="0.0 0.0 0.0" rpy="1.570795 0.0 0.0"/><geometry><cylinder length="0.1" radius="0.2"/></geometry></collision></link><!--連接右邊輪子和底座兩個links的關節joint，和之前思路一樣--><joint name="join_left_wheel" type="continuous"><parent link="base_link"/><child link="left_wheel"/><origin xyz="0.0 -0.3 0.0" rpy="0.0 0.0 0.0"/><axis xyz="0.0 1.0 0.0"/></joint><!--給urdf文件加入gazebo標簽，主要是加入對urdf中material中顏色的gazebo標簽--><gazebo reference="base_link"><!--指明這個標簽時針對base_link對象的--><material>Gazebo/Blue</material><!--將指定對象的顏色屬性轉換成gazebo能夠呈現的顏色，也就是所謂的gazebo標簽。因為之前設置的URDF中的顏色代碼在Gazebo中無法顯示。這和Gazebo的顯示顏色的機制有關--></gazebo><gazebo reference="right_wheel"><material>Gazebo/Black</material></gazebo><gazebo reference="left_wheel"><material>Gazebo/Black</material></gazebo><!--其余的代碼都可以直接轉成sdf格式以供gazebo使用，不用添加標簽--><!--這三個標簽最好寫在每個link結束后面。這里為了統一注釋，所以寫在了一起-->
</robot>

?這串代碼是使用 VSCode 寫的，因為 VSCode 里面有個專門的 URDF 插件，可以幫你補全代碼啥的，挺實用的。根據這個代碼我們就構建了一個擁有三個輪子的小車。然后用下面的 launch 文件（ launch 文件中可以寫中文注釋）打開rviz，將小車模型在rviz中顯示。

<launch<!--在通過命令行啟動launch文件時要提供那些參數，也可以設置這些參數的默認值--><arg name="model" /><arg name="gui" default="False" /><!--這個launch將在ＲＯＳ中的參數管理平臺中發布這些可以查看、使用的參數--><!--robot_description是最重要的！！他應該是能夠告訴 Rviz 應該讀取顯示哪個URDF文件--><param name="robot_description" textfile="$(find ros_robotics)/urdf/$(arg model)" /><param name="use_gui" value="$(arg gui)"/><!--use_gui表示是否打開一個可以控制整個模型中所有joint的ｇｕｉ界面--><!--這個launch要啟動３個ｎｏｄｅ節點: joint_state_publisher, robot_state_publisher and rviz--><!--下面啟動ｎｏｄｅ的方法其實是系統自動在命令窗口中分別用ｒｏｓｒｕｎ命令啟動各個node，以最后的ｎｏｄｅ代碼塊為例,它對應的啟動命令是：rosrun pkg(rviz) node(rviz) args(各個參數) required(意義不太清楚)  代碼塊中的name有著其他含義--><!--這段node代碼塊的含義：從pkg對應的功能包中，啟動名稱為type對應值的節點，并將啟動的階段在ＲＯＳ平臺中的名稱設為name的對應值--><!--joint_state_publisher將urdf中的關節信息發布出去，從而幫助rviz等軟件根據關節來顯示各個link和機器人模型--><node name="joint_state_publisher"pkg="joint_state_publisher"type="joint_state_publisher" /><!--robot_state_publisher會將機器人行動后的信息發布出去，幫助tf功能包確定機器人當前的狀態，從何在模擬軟件中顯示--><node name="robot_state_publisher"pkg="robot_state_publisher"type="robot_state_publisher" /><!--這段ｎｏｄｅ塊復雜一點：一開始的代碼和之前的意義一樣，即啟動ｒｖｉｚ。之后設置ｒｖｉｚ啟動時的所有參數args，和required(意義不明)urdf.rviz是ｒｖｉｚ軟件的可讀文件，它保存了上一次rviz退出時的狀態，之后啟動rviz時，通過讀取這個urdf.rviz可以恢復先前的狀態，并配置好rviz的參數--><node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotics)/urdf.rviz" required="true" />
</launch>

?直接用 roslaunch 命令執行該launch文件，顯示效果：

?第一次執行可能要調一下Rviz，先點左邊窗口的Add按鈕，找到RobotModel和TF，然后將Fixed Frame設定為base_link即可。
?其實也可以用Gazebo顯示，只要稍微修改一下就行，這就看個人的習慣了。這個小車模型可以直接在 Gazebo 中用Model Edit 搭建。相比于寫代碼，圖形化的搭建方式可能會更方便些。具體搭建方法大家可以參考這篇文章。

編寫控制小車移動的插件(與ROS交互)

?這個小車和常見的 turtlebot 不能說是一模一樣，但也可以說是毫無關系。不過好在Gazebo中有很多現成的插件和模型，我們可以自行選擇添加，編寫小車代碼只是為了幫助加深對 Gazebo 的理解。因為只是初步學習Gazebo仿真，所以我只做了控制小車移動的代碼。為了方便直接把代碼和注釋寫在一起：

#include <gazebo/gazebo.hh>//主要這個頭文件，包含 Gazebo 中的大部分對象
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <thread>
//ROS 要用到的所用到的頭文件
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>
//ROS 的消息文件
#include <std_msgs/Float32.h>
//所有 Gazebo 插件都要在 Gazebo 的命名空間下創建。Gazebo 是之前頭文件中已有的命名空間。
namespace gazebo
{/// \brief A plugin to control car motion.//可以把插件看作是C++中的類class PositionPlugin : public ModelPlugin//定義插件的類名，該類繼承 ModelPlugin{/// \brief Constructor 類對象構造函數，在gazebo中創建該插件時會執行一次public: PositionPlugin() {//一些測試代碼，顯示插件已經被構建了std::cout<<"Motion Plugin"<<std::endl;}/// \brief 在該插件被加載進模型中時，gazebo會運行一次 Load 函數/// \param[in] _model A pointer to the model that this plugin is　attached to.///  _model 指針參數會指向該插件所在的模型對象/// \param[in] _sdf A pointer to the plugin's SDF element./// _sdf指針指向插件對應的sdf元素。這種一般是指向world文件中編寫的插件的 SDF 代碼。public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){std::cout<<"Starting Load"<<std::endl;this->model = _model;std::cerr << "\n The model's name is [" <<_model->GetScopedName() << "]\n";std::string car_name_ori="Hello";//不好意思，這里把名字取的太隨意了，可以自行修改（T T）// Initialize ros, if it has not already bee initialized.if (!ros::isInitialized()){int argc = 0;char **argv = NULL;ros::init(argc, argv, car_name_ori + "_" + "node",ros::init_options::NoSigintHandler);}// 根據之前定義的名字，創建ROS節點this->rosNode.reset(new ros::NodeHandle(car_name_ori + "_" + "Handle"));// 創建一個ＲＯＳ中的ｔｏｐｉｃros::SubscribeOptions so =//create 函數創造使用相應信息數據類型的ｔｏｐｉｃros::SubscribeOptions::create<std_msgs::Float32>("/" + this->model->GetName() + "/" + car_name_ori + "/pos_cmd",//ｔｏｐｉｃ的名稱1,//ｔｏｐｉｃ在通道內保存信息的數量個數boost::bind(&PositionPlugin::OnRosMsg, this, _1),//該ｔｏｐｉｃ對應的回調函數ros::VoidPtr(), &this->rosQueue);//該ｔｏｐｉｃ的數據隊列是用戶自定義的，所以要將自己創建的 CallbackQueue 對象傳遞過去(引用傳遞)this->rosSub = this->rosNode->subscribe(so);//看到這里朋友們可以發現，該話題的創建方式和 ROS 教程中不同。上述這種方式最主要的點在于用戶要自行給定topic的隊列控制函數，即rosQueue。用戶要在函數中控制topic多久處理一次隊列中的信息。//將控制topic隊列的函數放進新的線程中，這樣才能使topic正常工作。this->rosQueueThread =std::thread(std::bind(&PositionPlugin::QueueThread, this));//將設置模型速度的函數和仿真世界刷新函數綁定，這樣才能有效改變模型速度（不斷試錯發現的）this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&PositionPlugin::OnUpdate, this));}/// \brief Handle an incoming message from ROS上述topic對應的回調函數/// \param[in] _msg A float value that is used to control the carpublic: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg){//std::cout<<"I can hear you "<<std::endl;this->vel=_msg->data;sleep(1);//加入這個會讓小車只運動一秒this->vel=0;}public:void OnUpdate(){	this->model->SetLinearVel(ignition::math::Vector3d(this->vel, 0, 0));//設置模型的線速度//this->model->SetAngularVel(ignition::math::Vector3d(this->vel, 0, 0));//設置模型的角速度//設置速度的函數要和仿真世界刷新函數共同執行，放在ROS回調函數中無效。}/// \brief ROS helper function that processes messagesprivate: void QueueThread()//這個函數是每多少timeout時間處理一次存儲在ｔｏｐｉｃ通道中的信息。這個函數和之前的創建topic的代碼要配套使用，否則topic會無法工作{static const double timeout = 0.01;while (this->rosNode->ok()){this->rosQueue.callAvailable(ros::WallDuration(timeout));//因為ｔｏｐｉｃ創建的方式比較特殊，故要設置數據隊列處理數據的時間間隔！！！！！！}}/// \brief Pointer to the model.private: physics::ModelPtr model;/// \brief A node use for ROS transportprivate: std::unique_ptr<ros::NodeHandle> rosNode;/// \brief A ROS subscriberprivate: ros::Subscriber rosSub;/// \brief A ROS callbackqueue that helps process messagesprivate: ros::CallbackQueue rosQueue;/// \brief A thread the keeps running the rosQueueprivate: std::thread rosQueueThread;private: double vel;//速度變量private: event::ConnectionPtr updateConnection;};// Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin. GZ_REGISTER_MODEL_PLUGIN(PositionPlugin)//這部分一定要有，這樣才能讓 gazebo 知道這是個插件
} // namespace gazebo

?為了讓 Gazebo 直接同時加載小車模型和編寫的插件，這里推薦寫一個 world 文件：

<?xml version="1.0" ?>
<sdf version="1.6"><world name="motion_world"><!-- 給world導入環境光模型 --><include><uri>model://sun</uri></include><!-- 地面 --><include><uri>model://ground_plane</uri></include><!-- A testing model --><model name='dd_robot'><include><uri>model://dd_robot</uri></include><!-- Attach the plugin to this model --><plugin name="motion_controller" filename="libmotion_controller.so"></plugin></model></world>
</sdf>

?然后用gazebo world文件命令運行這個文件。提一嘴，在執行命令前，我們要先把之前的小車 URDF 文件變成Gazebo 可用的SDF文件。方法很簡單，直接執行這個命令即可gz sdf -p urdf文件 > sdf文件。我們還需要為這個 SDF 文件配套寫一個 config 文件（介紹文件），內容如下：

<?xml version="1.0" encoding="UTF-8"?>
<model><name>dd_robot</name><version>1.0</version><sdf version="1.4">dd_robot.sdf</sdf><description>My Car.</description>
</model>

?然后我們把轉換好的 SDF 文件和 config 文件打包到一個叫 dd_robot 文件夾中，并將文件夾放進 ~/.gazebo/models/ 目錄下。只有這樣，在執行world文件時，Gazebo 才能找到你的模型文件，因為 Gazebo 是有默認模型搜索路徑的（也可以修改GAZEBO_MODEL_PATH環境變量，不過我沒試過）。然后先在一個命令窗口執行roscore，并在另一個窗口運行 world 文件。最后再在新的窗口向對應的topic發布命令：rostopic pub topicname 信息類型 信息，即左邊窗口的操作：

圖一、小車的原始位置。

圖二、控制小車以0.2m/s的速度移動1s鐘（這在代碼中有寫）

結尾

?在調試的過程中發現Gazebo還是很好玩的，不過它的機制很復雜，還常常報些搞不懂的錯（畢竟新手）。雖然Gazebo的模型庫中就有很多已經構建好的模型，但嘗試自己動手搭建未嘗不是件很刺激的事（也是件煩躁的事）。好了，對Gazebo的初步學習到此結束，之后還有很長的路要走，比如給模型加入紋理啥的。

總結

以上是生活随笔為你收集整理的Gazebo构建小车模型并通过ROS控制的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。