Gazebo构建小车模型并通过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 car
    public: 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 messages
    private: 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 transport
    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;

    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;

    /// \brief A thread the keeps running the rosQueue
    private: 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的初步学习到此结束，之后还有很长的路要走，比如给模型加入纹理啥的。