ROS：基于gazebo仿真運(yùn)行SLAM算法包（一）：在gazebo世界中啟動(dòng)機(jī)器人模型

前言

gazebo 中能仿真真實(shí)世界，包括很多物理屬性，比如慣性，碰撞等。對(duì)于沒有真實(shí)機(jī)器人和場(chǎng)地條件的情況下，作用十分強(qiáng)大。

gazebo 世界中的機(jī)器人

先上個(gè)成品圖：


robot_in_gazebo.png

在gazebo世界中，要完成仿真，首先要有機(jī)器人模型。

先貼上機(jī)器人模型的代碼：

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" 
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
    name="robot1_xacro">


    <xacro:property name="length_wheel" value="0.05" />
    <xacro:property name="radius_wheel" value="0.05" />
    <xacro:property name="camera_link" value="0.05" /> 
    <xacro:macro name="default_inertial" params="mass">
               <inertial>
                       <mass value="${mass}" />
                       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                                iyy="1.0" iyz="0.0"
                                izz="1.0" />
               </inertial>
    </xacro:macro>

    <!-- <link name="base_footprint">
        <visual>
            <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
        </visual>
        <xacro:default_inertial mass="0.0001"/>
    </link> -->
    <xacro:include filename="$(find robot1_description)/urdf/robot_depthCam_laser.gazebo" />
 
    <gazebo reference="base_footprint">
        <material>Gazebo/Green</material>
        <turnGravityOff>false</turnGravityOff>
    </gazebo>

    <!-- <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 0" />
        <parent link="base_footprint" />
        <child link="base_link" />
    </joint> -->


    <link name="base_link">
        <visual>
            <geometry>
                    <box size="0.2 .3 1"/>
                </geometry>
            <origin rpy="0 0 1.54" xyz="0 0 0.05"/>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <box size="0.2 .3 1"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <link name="wheel_1">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>

    <link name="wheel_2">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>

    </link>
    
    <link name="wheel_3">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->

            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>

    <link name="wheel_4">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
        <!--    <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0" />
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>

    </link>



 <joint name="base_to_wheel1" type="continuous">
   <parent link="base_link"/>
   <child link="wheel_1"/>
   <origin rpy="-1.5707 0 0" xyz="0.1 0.125 -0.45"/>
   <axis xyz="0 0 1" />
 </joint>

 <joint name="base_to_wheel2" type="continuous">
   <axis xyz="0 0 1" />
   <anchor xyz="0 0 0" />
   <limit effort="100" velocity="100" />
   <parent link="base_link"/>
   <child link="wheel_2"/>
   <origin rpy="-1.5707 0 0" xyz="-0.1 0.125 -0.45"/>
</joint>

 <joint name="base_to_wheel3" type="continuous">
   <parent link="base_link"/>
   <axis xyz="0 0 1" />
   <child link="wheel_3"/>
   <origin rpy="-1.5707 0 0" xyz="0.1 -0.125 -0.45"/>
 </joint>

 <joint name="base_to_wheel4" type="continuous">
   <parent link="base_link"/>
   <axis xyz="0 0 1" />
   <child link="wheel_4"/>
   <origin rpy="-1.5707 0 0" xyz="-0.1 -0.125 -0.45"/>
 </joint>
  <!-- Camera joint -->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0.125 0 0.575" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red">
    <color rgba="1 0 0 1"/>
      </material>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>



  <!-- Hokuyo joint -->
  <joint name="hokuyo_joint" type="fixed">
    <origin xyz="0.125 0.075 0.575" rpy="0 0 0"/>
    <parent link="base_link"/>
    <axis xyz="0 1 0" />
    <child link="hokuyo_link"/>
  </joint>

  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <!-- Drive controller -->
<gazebo>
  <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
    <updateRate>100.0</updateRate>
    <robotNamespace>/</robotNamespace>
    <leftFrontJoint>base_to_wheel1</leftFrontJoint>
    <rightFrontJoint>base_to_wheel3</rightFrontJoint>
    <leftRearJoint>base_to_wheel2</leftRearJoint>
    <rightRearJoint>base_to_wheel4</rightRearJoint>
    <wheelSeparation>4</wheelSeparation>
    <wheelDiameter>0.1</wheelDiameter>
    <robotBaseFrame>base_link</robotBaseFrame>
    <torque>1</torque>
    <topicName>cmd_vel</topicName>
    <broadcastTF>0</broadcastTF>
  </plugin>
</gazebo>


</robot>

這是一個(gè)帶有傳感器Hokuyo雷達(dá)，相機(jī)的機(jī)器人模型文件：robot_depthCam_laser.xacro，

<!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red">
    <color rgba="1 0 0 1"/>
      </material>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

這是相機(jī)模塊的節(jié)點(diǎn)，基本語法在這里就不多說了，很多資料有教程。

<!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

這是Hokuyo雷達(dá)的模塊。

<!-- Drive controller -->
<gazebo>
  <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
    <updateRate>100.0</updateRate>
    <robotNamespace>/</robotNamespace>
    <leftFrontJoint>base_to_wheel1</leftFrontJoint>
    <rightFrontJoint>base_to_wheel3</rightFrontJoint>
    <leftRearJoint>base_to_wheel2</leftRearJoint>
    <rightRearJoint>base_to_wheel4</rightRearJoint>
    <wheelSeparation>4</wheelSeparation>
    <wheelDiameter>0.1</wheelDiameter>
    <robotBaseFrame>base_link</robotBaseFrame>
    <torque>1</torque>
    <topicName>cmd_vel</topicName>
    <broadcastTF>0</broadcastTF>
  </plugin>
</gazebo>

這一個(gè)節(jié)點(diǎn)有點(diǎn)不同，這個(gè)是一個(gè)gazebo插件，是用于使機(jī)器人在gazebo世界中運(yùn)動(dòng)的模塊，正常來說，如果不需要在gazebo中進(jìn)行仿真運(yùn)動(dòng)，在一般的xacro文件或者urdf文件中，定義機(jī)器人是不需要這種模塊的，去掉這個(gè)模塊也可以在rviz中顯示出機(jī)器人模型。gazebo插件有很多種，這里要補(bǔ)充說明的是，上文中已經(jīng)添加了雷達(dá)，相機(jī)等模塊，但是也只是一個(gè)安放在機(jī)器人身上的一個(gè)模型，在gazebo中是不會(huì)起到仿真作用，因此必須要在另外一個(gè)文件（或者在同一個(gè)文件）中進(jìn)行插件定義。

定義gazebo插件：gazebo文件

在robot_depthCam_laser.gazebo文件中：

<?xml version="1.0"?>
<robot>
  <!-- materials -->
  <gazebo reference="base_link">
    <material>Gazebo/Orange</material>
  </gazebo>

 <gazebo reference="wheel_1">
        <material>Gazebo/Black</material>
 </gazebo>

 <gazebo reference="wheel_2">
        <material>Gazebo/Black</material>
 </gazebo>

 <gazebo reference="wheel_3">
        <material>Gazebo/Black</material>
 </gazebo>

 <gazebo reference="wheel_4">
        <material>Gazebo/Black</material>
 </gazebo>


  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/robot</robotNamespace>
    
    </plugin>
  </gazebo>

  <!-- 
  <gazebo reference="link1">
    <material>Gazebo/Orange</material>
  </gazebo>

  
  <gazebo reference="link2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="link3">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Orange</material>
  </gazebo>-->

  <!-- camera_link -->
  <gazebo reference="camera_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Red</material>
  </gazebo>

  <!-- hokuyo -->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/robot/laser/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
<gazebo reference="camera_link">
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${1.3962634}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName></cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_link</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>
</robot>

這個(gè)文件主要定義很多機(jī)器人在gazebo世界中各種表現(xiàn)，和屬性。這里主要看一下如何定義傳感器等插件的定義：

<!-- hokuyo -->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/robot/laser/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

這個(gè)是定義雷達(dá)模塊的，一般屬性定義成默認(rèn)值即可使用。

<gazebo reference="camera_link">
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${1.3962634}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName></cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_link</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>

這個(gè)是定義了深度相機(jī)模塊的屬性。
值得注意的是：


camera_link_reference.png

這里很重要，因?yàn)檫@里的reference作用是直接與xacro文件中的模塊名字進(jìn)行對(duì)應(yīng)，從而將相機(jī)屬性賦予xacro中camera_link模塊。


camera_link.png

這里在與gazebo世界中進(jìn)行仿真算法的時(shí)候十分重要，在這里定義的topic名稱必須與算法中所要求的topic名稱要一致。

啟動(dòng)gazebo世界和機(jī)器人模型

現(xiàn)在已經(jīng)完成了機(jī)器人模型的創(chuàng)建了，下一步就是把機(jī)器人在gazebo世紀(jì)中啟動(dòng)。在ros系統(tǒng)中，打開多個(gè)節(jié)點(diǎn)一般需要編寫launch文件：

<?xml version="1.0"?>
<launch>
  <arg name="world" />
  <arg name="model"/>

  <include file="$(find gazebo_ros)/launch/$(arg world)">
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="$(find xacro)/xacro.py --inorder $(arg model)"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

  <!-- joint state publisher -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>

  <!-- 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 robot1 -param robot_description -z 0.05"/>
  <!-- tf transform -->
    <arg name="pi/2" value="1.5707963267948966" />
    <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
    <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link camera_link 100" />

</launch>

launch文件的語法在這里也不多說，主要說以下使用這個(gè)launch 文件所需要的參數(shù)：


gazebo_wg.png

在啟動(dòng)的時(shí)候需要傳入兩個(gè)參數(shù)，一個(gè)是所用的gazebo環(huán)境的啟動(dòng)launch文件，一般會(huì)放在./opt/ros/melodic/share/gazebo_ros/launch中，包括自定義的gazebo世界的launch，最好也要放在同處地方。另一個(gè)參數(shù)則是所用的模型文件。這里傳入?yún)?shù)后會(huì)用到一個(gè)命令，完整命令是這樣的：

rosrun xacro xacro.py model.xacro > model_processed.urdf

這句命令很簡單，主要是用作將xacro文件導(dǎo)出成urdf文件。在launch文件中則是這樣使用：


gazebo_wg_robot.png

這里實(shí)際上是將xacro文件轉(zhuǎn)化為urdf后，將其放進(jìn)param參數(shù)服務(wù)器中，便于其他節(jié)點(diǎn)的調(diào)用，也就是launch文件中那個(gè)urdf_spawner節(jié)點(diǎn)的調(diào)用。


gazebo_wg_tf.png

在這里需要啟用一個(gè)節(jié)點(diǎn)是tf轉(zhuǎn)換，原因是在rtabmap算法中，世界坐標(biāo)系是x向前,y向左,z向上，但是在相機(jī)坐標(biāo)系中，三軸的方向是不同的，因此需要轉(zhuǎn)換。

啟動(dòng)gazebo世界和機(jī)器人模型：

roslaunch robot1_gazebo gazebo_wg.launch model:="`rospack find robot1_description`/urdf/robot_depthCam_laser.xacro" world:="willowgarage_world.launch"

啟動(dòng)后的結(jié)果：


gazebo_running.png

機(jī)器人已經(jīng)成功地在gazebo世界中運(yùn)行了，接下來就是運(yùn)行rtabmap算法進(jìn)行建圖了。

參考連接：http://gazebosim.org/tutorials?tut=ros_gzplugins

最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時(shí)請(qǐng)結(jié)合常識(shí)與多方信息審慎甄別。
平臺(tái)聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點(diǎn)，簡書系信息發(fā)布平臺(tái)，僅提供信息存儲(chǔ)服務(wù)。

友情鏈接更多精彩內(nèi)容