之前使用的是shenlan的源码实现了一系列的功能，那么根据之前所学习是否可以使用一个自己的机器人实现gazebo仿真。这里我们尝试一下：
1、按照之前的方式我们给自己的机器人添加碰撞属性以及惯性属性，机器人的xacro文件我们使用第六章的机器人。
2、给我们的机器人添加颜色属性，这里我先用的是同一种颜色。
3、为joint添加传动装置，注意这里的transmission是添加在主动轮下面的，代表驱动主动轮的运动。
4、添加一个控制器插件，插件放置在 <xacro:macro name=“mbot_base”>代码的最下面。
于是我们得到一个xacro文件：exper_gazebo.xacro：
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass" value="20" />
<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"/>
<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="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="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="${prefix}_wheel_link">
<material>Gazebo/Blue</material>
</gazebo>
<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>
<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 length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
<gazebo reference="${prefix}_caster_link">
<material>Gazebo/Blue</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>
<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_footprint">
<material>Gazebo/Blue</material>
</gazebo>
<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/Blue</material>
</gazebo>
<wheel prefix="left" reflect="-1"/>
<wheel prefix="right" reflect="1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
<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>
同样的，对于这个xacro文件我们建立一个调用的xacro文件：
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find exper4261)/xacro/exper_gazebo.xacro" />
<mbot_base/>
</robot>
然后我们使用一个launch文件启动这个xacro模型：
exper_gazebo.launch：
<launch>
<!-- 设置launch文件的参数 -->
<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="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>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find exper4261)/xacro/exper_gazebo2.xacro'" />
<!-- 运行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中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
这个文件来自于第十章中的launch文件。在这里我遇到过两个问题：
1.1、当时我启动launch文件时报错：
Unknown substitution command [mbot_description]. Valid commands are ['find', 'env', 'optenv', 'anon', 'arg']
The traceback for the exception was written to the log file
这里我感觉可能是复制的时候格式改变的问题，因为这的launch文件直接复制于章节十中的代码，对照视频中的资源应该是没有错的，但是我重新复制保存了一次后就没有报错了，有点奇怪。
1.2、当launch文件成功启动后，我能打开一个gazebo界面，但是机器人不显示，这里我们对机器人设置了颜色，按道理来说是会显示的。一开始以为是参数的问题，后来把参数修改了也不起作用，最后发现是宏定义的问题，注意到我们在第一个xacro中我将机器人主体定义为：mbot_xacro_gazebo，而我在调用的xacro文档中下面给出的是mbot_xacro，可能是这里冲突了导致找不到模型所以gazebo中没有成功显示我的模型，最后我将两者改成一样的就可以正常显示了，这时候我们可以看到这样一个模型：
刚打开的时候它是正的，但是慢慢会倾斜掉，这里因为我们的参数设置问题了，因为前面我把所有的碰撞属性以及惯性矩阵几乎都设置成了一样，所以我们尝试修改一下这些参数：
一开始我以为在footprint中我添加了碰撞属性导致的，后来删掉了但是没有什么改变，于是我开始修改其他的几个碰撞属性：将wheel和caster中的碰撞属性修改为：
<collision>
<origin xyz=" 0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
</geometry>
</collision>
以及：
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${caster_radius}"/>
</geometry>
</collision>
这时候我们的机器人就不会倾斜了。
但是仔细看会发现机器人还是悬浮的，同时这时候如果我们通过第十章中的命令去启动键盘节点：
$ roslaunch mbot_teleop mbot_teleop.launch
会发现其实机器人并不会运动。开始的时候我以为是惯性矩阵的问题，但是修改后发现没有效果，后来我想起来之前我对于footprint添加了碰撞属性以及惯性参数，我把这个删除了之后机器人就能正常显示并且运动了，这里也符合正常的逻辑思维，轨迹是不能有体积的，当轨迹有了体积参数后机器人本体位于轨迹之上就处于一种悬浮的状态。
然后我们将footprint的碰撞属性以及惯性矩阵删除就一切正常了。
然后同样的我给这个机器人加载一个地图，这个地图可以自己建好的也可以是源代码中的地图文件，怎么建地图在上一章里面已经介绍，这里我们添加一个源码中的playground给我们的launch文件。即将launch文件修改为：
<launch>
<!-- 设置launch文件的参数 -->
<arg name="world_name" value="$(find mbot_gazebo)/worlds/playground.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>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find exper4261)/xacro/exper_gazebo2.xacro'" />
<!-- 运行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中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
这时候我们可以看到gazebo中已经加载出一张正式的地图模型了，里面包含了一些基本的障碍物模型。
