gazebo构建仿真场景并制作机器人模型

仿真软件介绍与制作sdf模型文件：https://blog.youkuaiyun.com/kevin_chan04/article/details/78467218

构建仿真场景：https://blog.youkuaiyun.com/qq_36355662/article/details/80030372

编写xacro仿真模型文件和rviz launch文件：https://zhuanlan.zhihu.com/p/67741739

gazebo中构建一个仿真的地图非常的简单，因为他提供了很多的地图模型，可以直接拖到界面中使用，但是首先需要下载相关的模型：

1.模型下载

1、直接从终端下载，执行以下命令   

cd ~/.gazebo/

mkdir -p models
 cd ~/.gazebo/models/
 wget http://file.ncnynl.com/ros/gazebo_models.txt
 wget -i gazebo_models.txt
 ls model.tar.g* | xargs -n1 tar xzvf

2.利用百度网盘和国内官网下载

全部模型有大约200多M，但在线下载速度非常慢，晚上离开实验室早上来了就下完了。不过现在小伙伴已经将其共享至网盘，可直接下载，然后将其解压至~/.gazebo/models里。

链接：http://pan.baidu.com/s/1pKaeg0F 密码：cmxc （来自rosclub.cn）

百度网盘和国内官网并没有及时保持和官网同步更新，推荐大家还是通过官网下载，实在是慢的话可以通过百度网盘和ExBot ROS 专区下载。

下载后把文件放在.gazebo下的models

1.1制作简单的场景地图

制作简单的矩形场地可以直接进入 Edit-->Building Editor,然后选择墙壁拖拽即可，如图所示，编辑完成之后选择File-->save as-->选择目录，编辑名字，即可保存一个文件夹，里面有两个文件后缀名分别为.config和.sdf格式。再次点击File-->exit,点击确认之后退出，再次点击File->Save World As选择目录即可保存地图，地图格式为xxx.world。

2.制作机器人模型

2.1定义机器人主体部分

为了在 gazebo 中导入机器人模型，你需要先完成 URDF 模型。在这里我们将使用.xacro文件，虽然这可能更复杂，但是对于代码开发来说其功能非常强大。

我们首先将机器人的经常用到的值，使用变量进行定义，主要包括常用常数以及机器人属性：

    <!-- PROPERTY LIST --> 
    <xacro:property name="M_PI" value="3.1415926"/> 
    <!--圆柱体的定义需要长度length及半径radius-->   
  <xacro:property name="base_radius" value="0.20"/>    
 <xacro:property name="base_length" value="0.16"/>   
  <!--轮子也是一种圆柱体，同时要通过关节与主体连接 -->   
  <xacro:property name="wheel_radius" value="0.06"/>  
   <xacro:property name="wheel_length" value="0.025"/>   
  <!--关节的位置属性定义，由于关节需要将主体与轮子无缝连接，因此在x或者y方向上，一般定义为主体半径大小 --> 
    <xacro:property name="wheel_joint_y" value="0.19"/>     
<xacro:property name="wheel_joint_z" value="0.05"/>     
<!-- -->     <xacro:property name="caster_radius" value="0.015"/>       
<!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->     
<xacro:property name="caster_joint_x" value="0.18"/> 

接下来进行颜色定义：

 <!-- Defining the colors used in this robot -->     
<material name="yellow">
         <color rgba="1 0.4 0 1"/>
     </material>
     <material name="black"> 
        <color rgba="0 0 0 0.95"/>
     </material>     <material name="gray">
         <color rgba="0.75 0.75 0.75 1"/>
     </material> 

通过宏定义机器人的轮子，方便代码复用：

 

<xacro:macro name="wheel" 
params="prefix reflect"> 
<joint name="${prefix_wheel_joint} " type="continuous"> 
<!--continuous 表示是可以360度旋转的joint-->
     <origin xyz="0 ${reflect*wheel_joint_y} 
${-wheel_joint_z}" rpy="0 0 0"/> 
    <parent link="base_link"/>
     <child link="${prefix}_wheel_link"/>
     <axis xyz="0 1 0"/>
 </joint>
 <link name="${prefix}_wheel_link">
             <visual>
                 <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                 <geometry>
                     <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                 </geometry>
                 <material name="gray" />
             </visual> </link> </xacro:macro> 

定义机器人的支撑轮：

 <!-- Macro for robot caster --> 
    <xacro:macro name="caster" params="prefix reflect"> 
        <joint name="${prefix}_caster_joint" type="continuous">
         <!---(base_length/2 + caster_radius)表示一半在外面，一半在里面-->
             <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
             <parent link="base_link"/> 
            <child link="${prefix}_caster_link"/> 
            <axis xyz="0 1 0"/> 
        </joint> 
         <link name="${prefix}_caster_link"> 
            <visual> 
                <origin xyz="0 0 0" rpy="0 0 0"/> 
                <geometry>
                     <sphere radius="${caster_radius}" /> 
                </geometry>
                 <material name="black" /> 
            </visual>
         </link>
     </xacro:macro> 

定义机器人主体：

   <xacro:macro name="mbot_base">
             <link name="base_link">
             <visual>
                 <origin xyz=" 0 0 0" rpy="0 0 0" /> 
                <geometry>
                     <cylinder length="${base_length}" radius="${base_radius}"/> 
                </geometry> 
                <material name="yellow" /> 
            </visual> 
        </link> 
         <wheel prefix="left" reflect="-1"/>
         <wheel prefix="right" reflect="1"/>
          <caster prefix="front" reflect="-1"/>
         <caster prefix="back" reflect="1"/>     </xacro:macro>    

<visual></visual>中内容为在rviz 中显示相应的模型，collision中的内容为gazebo中的物理模型。

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
            </collision> 
            并为该关节设置物理惯性
            <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

设置机器人主体与地图的关联，地图为parent,主体味child,相对关系有origin xyz确定 

      <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

完整代码如下：

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- PROPERTY LIST -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="base_mass"   value="20" /> 
    <xacro:property name="base_radius" value="0.2"/>
    <xacro:property name="base_length" value="0.16"/>

    <xacro:property name="wheel_mass"   value="2" />
    <xacro:property name="wheel_radius" value="0.06"/>
    <xacro:property name="wheel_length" value="0.025"/>
    <xacro:property name="wheel_joint_y" value="0.19"/>
    <xacro:property name="wheel_joint_z" value="0.05"/>

    <xacro:property name="caster_mass"    value="0.5" /> 
    <xacro:property name="caster_radius"  value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
    <xacro:property name="caster_joint_x" value="0.18"/>

    <!-- Defining the colors used in this robot -->
    <material name="yellow">
        <color rgba="1 0.4 0 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 0.95"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>
    
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix  m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link>

        <gazebo reference="${prefix}_wheel_link">
            <material>Gazebo/Gray</material>
        </gazebo>

        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${prefix}_wheel_joint_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${prefix}_wheel_joint" >
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${prefix}_wheel_joint_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- Macro for robot caster -->
    <xacro:macro name="caster" params="prefix reflect">
        <joint name="${prefix}_caster_joint" type="continuous">
            <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_caster_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_caster_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="black" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
            </collision>      
            <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
        </link>

        <gazebo reference="${prefix}_caster_link">
            <material>Gazebo/Black</material>
        </gazebo>
    </xacro:macro>

    <xacro:macro name="mbot_base_gazebo">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>
        <gazebo reference="base_footprint">
            <turnGravityOff>false</turnGravityOff>
        </gazebo>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
            </collision>   
            <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

        <gazebo reference="base_link">
            <material>Gazebo/Yellow</material>
        </gazebo>

        <wheel prefix="left"  reflect="-1"/>
        <wheel prefix="right" reflect="1"/>

        <caster prefix="front" reflect="-1"/>
        <caster prefix="back"  reflect="1"/>

        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">
                <rosDebugLevel>Debug</rosDebugLevel>
                <publishWheelTF>true</publishWheelTF>
                <!--robotNamespace>/</robotNamespace-->
                <publishTf>1</publishTf>
                <publishWheelJointState>true</publishWheelJointState>
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <legacyMode>true</legacyMode>
                <leftJoint>left_wheel_joint</leftJoint>
                <rightJoint>right_wheel_joint</rightJoint>
                <wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>1</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
        </gazebo> 
    </xacro:macro>

</robot>

 2.2编写激光雷达模型文件lidar_gazebo.xacro文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">

    <xacro:macro name="rplidar" params="prefix:=laser">
        <!-- Create laser reference frame -->
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
                <material name="black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.06" radius="0.05"/>
                </geometry>
            </collision>
        </link>
        <gazebo reference="${prefix}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <sensor type="ray" name="rplidar">
                <pose>0 0 0 0 0 0</pose>
                <visualize>false</visualize>
                <update_rate>5.5</update_rate>
                <ray>
                    <scan>
                      <horizontal>
                        <samples>360</samples>
                        <resolution>1</resolution>
                        <min_angle>-3</min_angle>
                        <max_angle>3</max_angle>
                      </horizontal>
                    </scan>
                    <range>
                      <min>0.10</min>
                      <max>6.0</max>
                      <resolution>0.01</resolution>
                    </range>
                    <noise>
                      <type>gaussian</type>
                      <mean>0.0</mean>
                      <stddev>0.01</stddev>
                    </noise>
                </ray>
                <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
                    <topicName>scan</topicName>
                    <frameName>laser_link</frameName>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

2.3编写主文件mbot_with_laser_gazebo.xacro

<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mbot_description)/urdf/xacro/gazebo/mbot_base_gazebo.xacro" />
    <xacro:include filename="$(find mbot_description)/urdf/xacro/sensors/lidar_gazebo.xacro" />

    <xacro:property name="lidar_offset_x" value="0" />
    <xacro:property name="lidar_offset_y" value="0" />
    <xacro:property name="lidar_offset_z" value="0.16" />

    <!-- lidar -->
    <joint name="lidar_joint" type="fixed">
        <origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>

    <xacro:rplidar prefix="laser"/>

    <mbot_base_gazebo/>

</robot>

3.rviz launch文件编写

这里我们首先验证，其是否可以在rviz中进行显示，编写rviz的launch文件。

joint_state_publisher、robot_state_publisher、rviz是进行机器人仿真的必备节点。

   <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<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" />

    <!-- 运行rviz可视化界面 args="-d $(find mbot_sim)/config/mbot.rviz"-->
	<node name="rviz" pkg="rviz" type="rviz"  required="true" />

 

同时运行我们刚刚设定好的xacro文件以及关节控制插件：

	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_sim)/urdf/xacro/mbot.xacro'" />
	<arg name="gui" default="true" />
    <!--命令行参数，表示执行此命令-->
	<param name="robot_description" command="$(arg model)" />

    <!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="$(arg gui)"/>

rviz launch文件完整内容：mbot_laser_gazebo.launch 文件，并加载自己构建的地图rectangle2.world

<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_gazebo)/worlds/rectangle2.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 在gazebo中加载机器人模型-->

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_laser_gazebo.xacro'" /> 

    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mbot -param robot_description"/> 
</launch>