介绍

突然想试试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的初步学习到此结束，之后还有很长的路要走，比如给模型加入纹理啥的。