【ros学习】14.urdf、xacro机器人建模与rviz、gazebo仿真详解

一、起因

学校的这学期课程是ros机器人开发实战，我们学习小组也要搞一个自己的机器人模型，我们组又叫葫芦组，所以我就做了个葫芦形状的机器人，虽说有点丑，本来想用maya建模再导入的，奈何不太懂maya，于是乎就用基础三形状构建了这个机器人模型。

接下来我将会将urdf建模与gazebo仿真过程详细写出，共大家参考与互相学习，如有疏漏，敬请指正。
源码：hulu机器人源码-百度网盘提取码：hctm
不过这个葫芦机器人走路实在是不稳定，所以又写了另外的俩个机器人，源码和演示视频在文末。

二、机器人结构图

首先我们得把要构建的机器人画出来，把每个肢节的长宽高和半径都标注好，听说可以用CAD/SW/UG等软件画，我当时懒得画，画了个躯体就没画了，纯粹靠手调调节相对位置，耗费了不少时间，如果你会CAD可以减少很多调整时间，效果图大概是这样子的，找了个E100的图作为参考

三、准备工作

创建一个机器人建模的功能包

virtual-machine:~$ catkin_crate_pkg hulu_gazebo urdf xacro

依赖urdf、xacro功能包

创建各个放置文件的文件夹
总源码包文件联系结构如图：

config: 配置文件
launch：启动文件
meshes：dae模型文件
scripts：脚本文件
urdf：机器人模型文件
world：gazebo地图文件
CMakeLists.txt: 依赖及编译规则
package.xml: 包信息

修改Cmake编译系统的规则文件CMakeLists.txt，添加依赖

find_package(catkin REQUIRED COMPONENTSurdfxacrogazebo_pluginsgazebo_rosgazebo_ros_controlgeometry_msgsroscpprospy
)

修改软件包的描述文件package.xml

<buildtool_depend>catkin</buildtool_depend><!--编译依赖项 --><build_depend>urdf</build_depend><build_depend>xacro</build_depend><build_depend>gazebo_plugins</build_depend><build_depend>gazebo_ros</build_depend><build_depend>gazebo_ros_control</build_depend><build_depend>geometry_msgs</build_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><!-- 导出依赖项 --><build_export_depend>urdf</build_export_depend><build_export_depend>xacro</build_export_depend><build_export_depend>gazebo_plugins</build_export_depend><build_export_depend>gazebo_ros</build_export_depend><build_export_depend>gazebo_ros_control</build_export_depend><build_export_depend>geometry_msgs</build_export_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>rospy</build_export_depend><!-- 运行依赖项 --><exec_depend>urdf</exec_depend><exec_depend>xacro</exec_depend><exec_depend>gazebo_plugins</exec_depend><exec_depend>gazebo_ros</exec_depend><exec_depend>gazebo_ros_control</exec_depend><exec_depend>geometry_msgs</exec_depend><exec_depend>roscpp</exec_depend><exec_depend>rospy</exec_depend>

关于CMakeLists.txt可以参考我的这篇文章【ros学习】9.ros话题通讯之publisher与subscribe的c++实现详解

四、Arbotix的安装

由于rviz不像gazebo，没有提供控制器接口，想要让机器人在rviz中动起来，我们需要一款差速控制器，而ArbotiX这款控制电机、舵机的硬件控制板的ros功能包就提供了一个差速控制器，通过接收速度控制指令，更新机器人的里程计状态。

    $ git clone http://github.com/vanadiumlabs/arbotis_ros.git$ catkin_make

注意：arbotis_ros中的python文件需要添加可执行权限

五、xacro是什么？怎么用？

什么是Xacro? 我们可以把它理解成为针对URDF的扩展性和配置性而设计的宏语言(macro language)。有了Xacro，我们就可以像编程一样来写URDF文件。XACRO格式提供了一些更高级的方式来组织编辑机器人描述. 主要提供了三种方式来使得整个描述文件变得简单。

（1）Constants

<xacro:property name="WIDTH" value="2.0"/>

类似于C语言中的宏定义, 在头部定义后就可以${body_width}进行引用其数值，有了这个，至少我们可以把需要配置的变量进行统一管理和使用。

（2）Macros

<xacro:macro name="default_origin"><origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>
<xacro:default_origin />

Macros是xacro文件中最重要的部分. 就像宏函数一样, 完成一些最小模块的定义, 方便重用, 以及可以使用参数来标识不同的部分.

（3）Include

很多模型都是已宏的形式进行定义, 并以最小集团分成很多个文件. 而最终的机器人描述就变得非常简单了. 下面摘录一个ur5的描述文件. 从中可以看出来xacro的强大优势. 在最后的示例中我们还能够看到, urdf文件也是能够直接导入进来的.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5" ><!-- common stuff --><xacro:include filename="$(find ur_description)/urdf/ur5/common.gazebo.xacro" /><!-- ur5 --><xacro:include filename="$(find ur_description)/urdf/ur5/ur5.urdf.xacro" /><!-- arm --><xacro:ur5_robot prefix="" joint_limited="false"/><link name="world" /><joint name="world_joint" type="fixed"><parent link="world" /><child link = "base_link" /><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></joint></robot>

类似于C语言中的include, 先将该文件扩展到包含的位置. 但包含进来的文件很有可能只是一个参数宏的定义. 并没有被调用.\quad

举例说明打开新的终端，输入roslaunch urdf_demo display_xacro.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成。

（4）数学运算

六、URDF所有的标签解析

主体是俩个标签，joint和link标签，joint代表关节,link代表肢节。transmission代表变速器，他控制关节的转动，gazebo设置gazebo相关参数。官方文档：XML Specifications

joint:关节

origin:
- xyz 代表孩子肢节相对与父肢节坐标系的相对位移，x相对父坐标系x轴原点前后移动，以此类推yz。
- rpy 代表该关节的横滚角-roll、俯仰角-pitch、偏航角-yaw
一般定义载体的右、前、上三个方向构成右手系，绕向前的轴[x轴]旋转就是横滚角，绕向右的轴[y轴]旋转就是俯仰角，绕向上的轴[z轴]旋转就是航向角

逆时针为正，比如rpy=“1.5705 0 0”,代表该关节及关节上绑定的其他关节作为一个整体绕关节的x轴逆时针旋转90度，为什么是1.5705呢？因为是用弧度来表示的，一度等于0.01745弧度，不过也取决于你的π的精度，比如3.14159/180=0.01745结果就是一弧度的大小,
parent：
- 父肢节
child:
- 子肢节
axis：
- 旋转轴，在关节框架中指定的关节轴。这是旋转关节的旋转轴，棱柱形关节的平移轴，平面关节的表面法线。轴在参考关节框架中指定。固定关节和浮动关节不使用轴字段。
joint的属性type：关节的类型
-

link:肢节

visual：视觉，描述机器人肢节部分的外观
- origin 视觉元素的参考框架相对于肢节的参考框架。和上面关节的 origin一样的意思。
- geometry （必需）视觉对象的形状。这可以是一个如下：

<geometry><box> 盒子，长方体、正方体、立方体<box size="${x} ${y} ${z}"/>size属性包含盒子的三个边长。盒子的原点在中间。<cylinder> 气缸，其实就是个圆柱体<cylinder radius="0.02" length="0.3"/>指定半径和长度。圆柱体的原点位于其中心。<sphere> 球体<sphere radius="0.05" />指定半径。球体的原点位于其中心。<mesh>  网格，导入通过其他建模软件建的dae文件<mesh filename="package://hulu_gazebo/meshes/kinect.dae" />由filename指定的trimesh元素，以及用于缩放网格的axis-aligned-bounding-box的可选比例。任何几何格式都是可以接受的，但是特定的应用程序兼容性取决于实现。最佳的纹理和颜色支持的推荐格式是Collada .dae文件。不能在引用相同模型的机器之间传输网格文件。它必须是本地文件。
</geometry>

material：材料
视觉元素的材料。可以在顶级“机器人”元素中指定“链接”对象之外的材料元素。然后，您可以从链接元素中按名称引用材料。

<material><color> （可选）<color rgba="1 0.27 0 1"/>rgba由一组代表红色/绿色/蓝色/ alpha的四个数字指定的材料的颜色，每个数字的范围为[0,1]。最后的那个参数是透明度，0全透明，1不透明rgb(207 166 87 1)  -> rgba(207/255 166/255 87/255 1)<texture> （可选）材质的纹理由文件名指定
</material>

inertial：惯性参数
- Gazebo的官方文档明确指出，必须正确指定和配置每个<link>元素中的<inertial>元素，urdf模型才能在gazebo中正常工作，因为gazebo里使用的文件格式并不是urdf格式，而是将urdf转换为它自己独有的的SDF格式再进行仿真。
- 背景

尽管URDF在ROS中是一种有用且标准化的格式，但它们缺少许多功能，并且尚未进行更新以应对机器人技术的不断发展的需求。URDF只能单独指定单个机器人的运动学和动力学特性。URDF无法指定世界中机器人本身的姿势。它也不是通用的描述格式，因为它不能指定关节环（平行连接），并且缺乏摩擦和其他特性。此外，它不能指定不是机器人的东西，例如灯光，高度图等。
在实现方面，URDF语法大量使用XML属性破坏了正确的格式设置，这反过来又使URDF更加不灵活。也没有向后兼容的机制。
为了解决此问题，创建了一种称为仿真描述格式（SDF）的新格式
，供凉亭使用，以解决URDF的缺点。SDF是从世界级到机器人级的所有内容的完整描述。它具有可伸缩性，并易于添加和修改元素。SDF格式本身使用XML进行描述，这有助于使用简单的升级工具将旧版本迁移到新版本。它也是自我描述的。
作者的意图是使URDF尽可能完整地记录在凉亭中，并在凉亭中提供支持，但与读者相关的是要了解为什么存在两种格式以及两者都有缺点。如果在URDF中进行更多工作以将其更新为当前的机器人技术，那将是很好的。

    - origin   这是惯性参考系相对于肢节参考系的姿势。- 惯性参考系的原点必须位于重心。惯性基准帧的轴线做不需要与惯性的主轴对齐。

</inertial><mass value="${m}" />肢节的质量由此元素的value属性表示<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>

<!-- Macro for inertia matrix --><!-- 立方体惯性矩阵 --><xacro:macro name="box_inertial_matrix" params="m l w h"><inertial><mass value="${m}" /><inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"iyy="${m*(w*w + l*l)/12}" iyz= "0"izz="${m*(w*w + h*h)/12}" /></inertial></xacro:macro><!-- 球体惯性矩阵 --><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>

矩阵原理及计算方法：机器人的惯性、视觉、碰撞特征计算与表示

collision：碰撞属性
- origin 碰撞元素的参考框架相对于肢节的参考框架。
- geometry （必需）碰撞对象的形状。

<collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><box size="0.1 0.1 0.01"/></geometry>
</collision>

这个元素是表明机器人的虚拟预留碰撞范围的，视觉上是绿色的范围是模型，但实际给肢节的碰撞范围比视觉效果大，碰撞范围是红框框，以避免机器人真的撞墙，不过经常为了简便，就直接复制一遍visual的origin、geometry就行了

transmission:变速器

这个我也不是很懂，大意就是为你的轮子的关节选择一种gazebo提供的电机进行驱动，然后电机会订阅cmd_vel话题去接收速度控制指令然后控制你绑定的关节按axis旋转轴转动。

   1个 <transmission name =“ simple_trans”> 2   <type> transmission_interface / SimpleTransmission </ type> 3   <joint name =“ foo_joint”> 4     <hardwareInterface> EffortJointInterface </ hardwareInterface> 5   </ joint> 6   <actuator name =“ foo_motor”> 7     <mechanicalReduction> 50 </ mechanicalReduction> 8     <hardwareInterface> EffortJointInterface </ hardwareInterface> 9   </ actuator> 10 </ transmission>

<transmission><type> (one occurrence)指定传输类型。<joint> (one or more occurrences)变速箱所连接的关节。<hardwareInterface> (one or more occurrences)指定支持的联合空间硬件接口。<actuator> (one or more occurrences)变速器连接到的执行器。（电机）<mechanicalReduction> (optional)指定关节/执行器传动机构的机械减速。<hardwareInterface> (optional) (one or more occurrences)指定支持的联合空间硬件接口。</actuator>
</transmission>

gazebo:`<gazebo>`元素是URDF的扩展，用于指定在gazebo中进行仿真所需的其他属性。

1.可选择的<gazebo>标签添加方式

· 为<link>元素添加一个<gazebo>元素1.将视觉颜色转换为凉亭格式2.将Stl文件转换为dae文件以获得更好的纹理3.添加传感器插件
· 为<joint>元素添加一个<gazebo>元素1.设置适当的阻尼动力学2.添加执行器控制插件
· 为<robot>元素添加一个<gazebo>元素
· 如果应该将机器人牢固地附加到world / base_link，请添加链接<link name="world"/>

link内的可选属性，以后有空再翻译成中文
joint内的可选属性
官方文档：教程：在gazebo中使用URDF
gazebo插件
gazebo插件为您的URDF模型提供了更大的功能，并且可以将ROS消息和服务呼叫联系在一起，以实现传感器输出和电动机输入。

差动驱动
说明：模型插件，为凉亭中的差动驱动机器人提供基本控制器。您需要一个定义明确的差动驱动器机器人才能使用此插件。
<gazebo><plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so"><!-- Plugin update rate in Hz --><!--插件更新率（Hz）--><updateRate>${update_rate}</updateRate><!-- Name of left joint, defaults to `left_joint` --><!--左关节的名称，默认为“左关节”--><leftJoint>base_link_left_wheel_joint</leftJoint><!-- Name of right joint, defaults to `right_joint` --><!--右关节的名称，默认为“右关节”--><rightJoint>base_link_right_wheel_joint</rightJoint><!-- The distance from the center of one wheel to the other, in meters, defaults to 0.34 m --><!--从一个车轮中心到另一个车轮中心的距离（以米为单位）默认为0.34米--><wheelSeparation>0.5380</wheelSeparation><!-- Diameter of the wheels, in meters, defaults to 0.15 m --><!--车轮直径（以米为单位）默认为0.15米--><wheelDiameter>0.2410</wheelDiameter><!-- Wheel acceleration, in rad/s^2, defaults to 0.0 rad/s^2 --><!--车轮加速度，单位为rad/s^2，默认为0.0 rad/s^2--><wheelAcceleration>1.0</wheelAcceleration><!-- Maximum torque which the wheels can produce, in Nm, defaults to 5 Nm --><!--车轮可产生的最大扭矩（Nm）默认为5 Nm--><wheelTorque>20</wheelTorque><!-- Topic to receive geometry_msgs/Twist message commands, defaults to `cmd_vel` --><!--主题接收几何图形/扭曲消息命令，默认为“cmd”级别--><commandTopic>cmd_vel</commandTopic><!-- Topic to publish nav_msgs/Odometry messages, defaults to `odom` --><!--主题发布导航消息/里程计消息，默认为“odom”--><odometryTopic>odom</odometryTopic><!-- Odometry frame, defaults to `odom` --><!--里程计帧，默认为“odom”--><odometryFrame>odom</odometryFrame><!-- Robot frame to calculate odometry from, defaults to `base_footprint` --><!--要从中计算里程表的Robot帧，默认为“base_footprint”--><robotBaseFrame>base_footprint</robotBaseFrame><!-- Odometry source, 0 for ENCODER, 1 for WORLD, defaults to WORLD --><!--里程计源，0代表编码器，1代表世界，默认为世界--><odometrySource>1</odometrySource><!-- Set to true to publish transforms for the wheel links, defaults to false --><!--设置为true可发布控制盘链接的变换，默认值为false--><publishWheelTF>true</publishWheelTF><!-- Set to true to publish transforms for the odometry, defaults to true --><!--设置为true以发布里程计的变换，默认为true--><publishOdom>true</publishOdom><!-- Set to true to publish sensor_msgs/JointState on /joint_states for the wheel joints, defaults to false --><!--如果设置为true，则发布车轮接头的sensor_msgs/JointState on/joint_状态，则默认为false--><publishWheelJointState>true</publishWheelJointState><!-- Set to true to swap right and left wheels, defaults to true --><!--设置为“真”可交换左右控制盘，默认为“真--><legacyMode>false</legacyMode></plugin>
</gazebo>

插件使用具体教程：Tutorial: Using Gazebo plugins with ROS

七、urdf结合xacro构建机器人并书写launch文件

模型文件及启动文件联系结构如图：

（1）机器人主体模型 hulu_base_description.xacro

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro"><!-- PROPERTY LIST --><xacro:property name="M_PI" value="3.14159265354"/><!-- body sphere --><xacro:property name="body_mass"   value="20" /><xacro:property name="body_radius" value="0.20"/><xacro:property name="body_length" value="0.16"/><!-- head sphere --><xacro:property name="head_mass"   value="5" /><xacro:property name="head_radius" value="0.1"/><!-- wheel cylinder --><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_x" value="0.15"/><xacro:property name="wheel_joint_y" value="0.15"/><xacro:property name="wheel_joint_z" value="0.16"/><!-- caster sphere --><xacro:property name="caster_mass"    value="0.5" /> <xacro:property name="caster_radius"  value="0.06"/><xacro:property name="caster_joint_x" value="0.1"/><xacro:property name="caster_joint_y" value="0.1"/><xacro:property name="caster_joint_z" value="0.16"/><!-- ears box --><xacro:property name="ears_mass" value="0.02" /><xacro:property name="ears_x" value="0.008" /><xacro:property name="ears_y" value="0.008" /><xacro:property name="ears_z" value="0.04" /><xacro:property name="ears_joint_y" value="0.099"/><xacro:property name="ears_joint_z" value="0.02"/><!-- eye sphere --><xacro:property name="eye_mass" value="0.02" /><xacro:property name="eye_radius" value="0.01" /><xacro:property name="eye_joint_x" value="0.085"/><xacro:property name="eye_joint_y" value="0.05"/><xacro:property name="eye_joint_z" value="0.02"/><!-- eyeball sphere --><xacro:property name="eyeball_mass" value="0.02" /><xacro:property name="eyeball_radius" value="0.0025" /><xacro:property name="eyeball_joint_x" value="0.0095"/><!-- 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><material name="white"><color rgba="1 1 1 1"/></material><material name="orange"><color rgba="1 0.27 0 1"/></material><!-- Macro for inertia matrix --><xacro:macro name="box_inertial_matrix" params="m l w h"><inertial><mass value="${m}" /><inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"iyy="${m*(w*w + l*l)/12}" iyz= "0"izz="${m*(w*w + h*h)/12}" /></inertial></xacro:macro><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="body_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><!-- Specifies the color of the limb in gazebo --><gazebo reference="${prefix}_wheel_link"><material>Gazebo/DarkGrey</material></gazebo><!-- electric machinery --><!-- 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 ${-caster_joint_z}" rpy="0 0 0"/><parent link="body_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/GreyTransparent</material></gazebo></xacro:macro><!-- Macro for robot ears --><xacro:macro name="ears" params="prefix reflect" ><joint name="${prefix}_ears_joint" type="fixed"><origin xyz="0 ${reflect*ears_joint_y} ${ears_joint_z}" rpy="0 0 0" /><parent link="head_link"/><child link="${prefix}_ears_link" /></joint><link name="${prefix}_ears_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><box size="${ears_x} ${ears_y} ${ears_z}"/></geometry><material name="yellow" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><box size="${ears_x} ${ears_y} ${ears_z}"/></geometry></collision><box_inertial_matrix m="${ears_mass}" l="${ears_x}" w="${ears_y}" h="${ears_z}" /></link><gazebo reference="${prefix}_ears_link"><material>Gazebo/Orange</material></gazebo></xacro:macro><!-- Macro for robot eye --><xacro:macro name="eye" params="prefix reflect"><joint name="${prefix}_eye_joint" type="fixed"><origin xyz="${eye_joint_x} ${reflect*eye_joint_y} ${eye_joint_z}" rpy="0 0 0" /><parent link="head_link"/><child link="${prefix}_eye_link" /></joint><link name="${prefix}_eye_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${eye_radius}" /></geometry><material name="white" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${eye_radius}" /></geometry></collision><sphere_inertial_matrix  m="${eye_mass}" r="${eye_radius}" /></link><gazebo reference="${prefix}_eye_link"><material>Gazebo/White</material></gazebo></xacro:macro><!-- Macro for robot eyeball --><xacro:macro name="eyeball" params="prefix"><joint name="${prefix}_eyeball_link_joint" type="fixed"><origin xyz="${eyeball_joint_x} 0 0" rpy="0 0 0" /><parent link="${prefix}_eye_link"/><child link="${prefix}_eyeball_link" /></joint><link name="${prefix}_eyeball_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${eyeball_radius}"/></geometry><material name="black" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${eyeball_radius}"/></geometry></collision><sphere_inertial_matrix  m="${0.02}" r="${eyeball_radius}" /></link><gazebo reference="${prefix}_eyeball_link"><material>Gazebo/Black</material></gazebo></xacro:macro><!-- Macro for robot --><xacro:macro name="hulu_body_gazebo"><!-- shadow , odom reference resources--><link name="body_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="body_footprint"><!-- cutdown gravuity --><turnGravityOff>false</turnGravityOff></gazebo><!-- body joint --><joint name="body_footprint_joint" type="fixed"><origin xyz="0 0 ${body_length/2 + caster_radius*2}" rpy="0 0 0" /><parent link="body_footprint"/><child link="body_link" /></joint><!-- body --><link name="body_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${body_radius}"/></geometry><material name="orange" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${body_radius}"/></geometry></collision>   <sphere_inertial_matrix  m="${body_mass}" r="${body_radius}" /></link><gazebo reference="body_link"><material>Gazebo/RedBright</material></gazebo><joint name="body_link_joint" type="fixed"><origin xyz="0 0 0.28" rpy="0 0 0" /><parent link="body_link"/><child link="head_link" /></joint><!-- head --><link name="head_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${head_radius}"/></geometry><material name="orange" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><sphere radius="${head_radius}"/></geometry></collision><sphere_inertial_matrix  m="${head_mass}" r="${head_radius}" /></link><gazebo reference="head_link"><material>Gazebo/RedBright</material></gazebo><joint name="hand_link_joint" type="fixed"><origin xyz="0 -0.19 0.1" rpy="0 1.4 -1.5705" /><parent link="body_link"/><child link="hand_link" /></joint><!-- hand --><link name="hand_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder radius="0.02" length="0.3"/></geometry><material name="yellow" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder radius="0.02" length="0.3"/></geometry></collision><cylinder_inertial_matrix  m="0.05" r="0.02" h="0.3"/></link><gazebo reference="hand_link"><material>Gazebo/RedBright</material></gazebo><joint name="palm_link_joint" type="fixed"><origin xyz="0 0 0.2" rpy="1.75 0 1.5705" /><parent link="hand_link"/><child link="palm_link" /></joint><!-- palm --><link name="palm_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><box size="0.1 0.1 0.01"/></geometry><material name="yellow" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><box size="0.1 0.1 0.01"/></geometry></collision><box_inertial_matrix m="0.1" l="0.1" w="0.1" h="0.01" /></link><gazebo reference="palm_link"><material>Gazebo/RedBright</material></gazebo><wheel prefix="left"  reflect="-1"/><wheel prefix="right" reflect="1"/><caster prefix="front" reflect="-1"/><caster prefix="back"  reflect="1"/><eye prefix="left" reflect="-1" /><eye prefix="right" reflect="1" /><ears prefix="left" reflect="-1" /><ears prefix="right" reflect="1" /><eyeball prefix="left"/><eyeball prefix="right"/><!-- 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>body_footprint</robotBaseFrame></plugin></gazebo> </xacro:macro></robot>

（2）机器人传感器的模型

照相机 camera_gazebo.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera"><xacro:macro name="usb_camera" params="prefix:=camera"><!-- 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><box size="0.01 0.02 0.02" /></geometry><material name="black"/></visual><collision><origin xyz="0.0 0.0 0.0" rpy="0 0 0" /><geometry><box size="0.01 0.02 0.02" /></geometry></collision></link><gazebo reference="${prefix}_link"><material>Gazebo/Black</material></gazebo><gazebo reference="${prefix}_link"><sensor type="camera" name="camera_node"><update_rate>30.0</update_rate><camera name="head"><horizontal_fov>1.3962634</horizontal_fov><image><width>1280</width><height>720</height><format>R8G8B8</format></image><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="gazebo_camera" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><updateRate>0.0</updateRate><cameraName>/camera</cameraName><imageTopicName>image_raw</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>camera_link</frameName><hackBaseline>0.07</hackBaseline><distortionK1>0.0</distortionK1><distortionK2>0.0</distortionK2><distortionK3>0.0</distortionK3><distortionT1>0.0</distortionT1><distortionT2>0.0</distortionT2></plugin></sensor></gazebo></xacro:macro>
</robot>

3D体感摄影机 kinect kinect_gazebo.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera"><xacro:macro name="kinect_camera" params="prefix:=camera"><!-- Create kinect reference frame --><!-- Add mesh for kinect --><link name="${prefix}_link"><origin xyz="0 0 0" rpy="0 0 0"/><visual><origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/><geometry><mesh filename="package://hulu_gazebo/meshes/kinect.dae" /></geometry></visual><collision><geometry><box size="0.07 0.3 0.09"/></geometry></collision></link><joint name="${prefix}_optical_joint" type="fixed"><origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/><parent link="${prefix}_link"/><child link="${prefix}_frame_optical"/></joint><link name="${prefix}_frame_optical"/><gazebo reference="${prefix}_link"><sensor type="depth" name="${prefix}"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*M_PI/180.0}</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_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>${prefix}</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>${prefix}_frame_optical</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></xacro:macro>
</robot>

激光雷达 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.005"/></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>

（3）机器人主体添加传感器后的模型 hulu_with_sensors.xacro

<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro"><xacro:include filename="$(find hulu_gazebo)/urdf/xacro/gazebo/hulu_base_description.xacro" /><xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/camera_gazebo.xacro" /><xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/lidar_gazebo.xacro" /><xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/kinect_gazebo.xacro" /><xacro:property name="camera_offset_x" value="0.082" /><xacro:property name="camera_offset_y" value="0" /><xacro:property name="camera_offset_z" value="0.06" /><xacro:property name="lidar_offset_x" value="0" /><xacro:property name="lidar_offset_y" value="0" /><xacro:property name="lidar_offset_z" value="0.125" /><xacro:property name="kinect_offset_x" value="0" /><xacro:property name="kinect_offset_y" value="0" /><xacro:property name="kinect_offset_z" value="-0.03" /><!-- Camera --><joint name="camera_joint" type="fixed"><origin xyz="${camera_offset_x} ${camera_offset_y} ${-camera_offset_z}" rpy="0 0.27483 0" /><parent link="head_link"/><child link="camera_link"/></joint><xacro:usb_camera prefix="camera"/><!-- lidar --><joint name="lidar_joint" type="fixed"><origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" /><parent link="head_link"/><child link="laser_link"/></joint><xacro:rplidar prefix="laser"/><!-- kinect --><joint name="kinect_joint" type="fixed"><origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="-3.14159 0 0" /><parent link="palm_link"/><child link="kinect_link"/></joint><xacro:kinect_camera prefix="kinect"/><hulu_body_gazebo/></robot>

（4）创建arbotix与rviz的配置文件

arbotix配置文件 fake_hulu_arbotix.yaml

controllers: {base_controller: {type: diff_controller, base_frame_id: body_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}

rviz配置文件 hulu_arbotix.rviz

Panels:- Class: rviz/DisplaysHelp Height: 78Name: DisplaysProperty Tree Widget:Expanded:- /Global Options1- /Status1- /Odometry1/Shape1Splitter Ratio: 0.5Tree Height: 631- Class: rviz/SelectionName: Selection- Class: rviz/Tool PropertiesExpanded:- /2D Pose Estimate1- /2D Nav Goal1- /Publish Point1Name: Tool PropertiesSplitter Ratio: 0.588679016- Class: rviz/ViewsExpanded:- /Current View1Name: ViewsSplitter Ratio: 0.5- Class: rviz/TimeExperimental: falseName: TimeSyncMode: 0SyncSource: ""
Visualization Manager:Class: ""Displays:- Alpha: 0.5Cell Size: 1Class: rviz/GridColor: 160; 160; 164Enabled: true
省略以下没有营养的内容

（5）创建在rviz进行仿真的launch文件 arbotix_hulu_with_rviz.launch

<launch><arg name="model" default="$(find xacro)/xacro --inorder '$(find hulu_gazebo)/urdf/xacro/gazebo/hulu_with_sensors.xacro'" /><arg name="gui" default="false" /><param name="robot_description" command="$(arg model)" /><!-- 设置GUI参数，显示关节控制插件 --><param name="use_gui" value="$(arg gui)"/><node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen"><rosparam file="$(find hulu_gazebo)/config/fake_hulu_arbotix.yaml" command="load" /><param name="sim" value="true"/></node><!-- 运行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可视化界面 --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find hulu_gazebo)/config/hulu_arbotix.rviz" required="true" /></launch>

（6）创建在gazebo进行仿真的launch文件 view_hulu_with__gazebo.launch

<launch><!-- 设置launch文件的参数 --><arg name="world_name" value="$(find hulu_gazebo)/worlds/maze.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 hulu_gazebo)/urdf/xacro/gazebo/hulu_with_sensors.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 hulu -param robot_description"/> </launch>

（6）创建键盘发布控制指令的launch文件 hulu_teleop.launch

<launch><node name="hulu_teleop" pkg="hulu_gazebo" type="hulu_teleop.py" output="screen"><param name="scale_linear" value="0.1" type="double"/><param name="scale_angular" value="0.4" type="double"/></node>
</launch>

（7）键盘速度控制脚本文件 hulu_teleop.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, ttymsg = """
Control hulu!
---------------------------
Moving around:u    i    oj    k    lm    ,    .q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothlyCTRL-C to quit
"""moveBindings = {'i':(1,0),'o':(1,-1),'j':(0,1),'l':(0,-1),'u':(1,1),',':(-1,0),'.':(-1,1),'m':(-1,-1),}speedBindings={'q':(1.1,1.1),'z':(.9,.9),'w':(1.1,1),'x':(.9,1),'e':(1,1.1),'c':(1,.9),}def getKey():tty.setraw(sys.stdin.fileno())rlist, _, _ = select.select([sys.stdin], [], [], 0.1)if rlist:key = sys.stdin.read(1)else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keyspeed = .2
turn = 1def vels(speed,turn):return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)rospy.init_node('hulu_teleop')pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)x = 0th = 0status = 0count = 0acc = 0.1target_speed = 0target_turn = 0control_speed = 0control_turn = 0try:print msgprint vels(speed,turn)while(1):key = getKey()# 运动控制方向键（1：正方向，-1负方向）if key in moveBindings.keys():x = moveBindings[key][0]th = moveBindings[key][1]count = 0# 速度修改键elif key in speedBindings.keys():speed = speed * speedBindings[key][0]  # 线速度增加0.1倍turn = turn * speedBindings[key][1]    # 角速度增加0.1倍count = 0print vels(speed,turn)if (status == 14):print msgstatus = (status + 1) % 15# 停止键elif key == ' ' or key == 'k' :x = 0th = 0control_speed = 0control_turn = 0else:count = count + 1if count > 4:x = 0th = 0if (key == '\x03'):break# 目标速度=速度值*方向值target_speed = speed * xtarget_turn = turn * th# 速度限位，防止速度增减过快if target_speed > control_speed:control_speed = min( target_speed, control_speed + 0.02 )elif target_speed < control_speed:control_speed = max( target_speed, control_speed - 0.02 )else:control_speed = target_speedif target_turn > control_turn:control_turn = min( target_turn, control_turn + 0.1 )elif target_turn < control_turn:control_turn = max( target_turn, control_turn - 0.1 )else:control_turn = target_turn# 创建并发布twist消息twist = Twist()twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turnpub.publish(twist)except:print efinally:twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.publish(twist)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

八、仿真演示

1.rviz展示模型

(1)直接启动
$ roslaunch hulu_gazebo arbotix_hulu_with_rviz.launch

(2)启动键盘控制节点
$ roslaunch hulu_gazebo hulu_teleop.launch

2.创建自己的gazebo地图

3.gazebo仿真

(1) 摄像头仿真
       启动gazebo环境
      $ roslaunch hulu_gazebo view_hulu_with_gazebo.launch
       查看当前ROS计算图
       $ rqt_graph

      查看当前发布的话题
      $ rostopic list

打开摄像头
       $ rqt_image_view
      打开键盘操控节点
      $ roslaunch hulu_gazebo hulu_teleop.launch
(2) 激光雷达仿真
      打开rviz
      $ rosrun rviz rviz
      固定坐标系选择laser_link肢节

      添加一个LaserScan模块，订阅/scan话题

      将激光点尺寸改大点方便看

      键盘控制移动

      由于我的激光雷达装在葫芦娃的头顶，所以比它头矮的物品他都扫描不到，这就很尴尬
(3) kinect仿真
       把rviz关掉再重进比较快

固定坐标系选择kinect_link肢节
      添加一个piontcloud2模块，订阅/kinect/depth/points话题

      添加一个image模块，订阅/kinect/rgb/image_raw话题

      键盘控制移动

      查看当前ROS计算图
       $ rqt_graph

九、后记

测试发现这个葫芦机器人走路很不稳定，基本有坎或者撞墙就翻车，而且它的激光雷达扫描不到比自己矮的物体，调整激光雷达的扫描角度也扫不到，可能是被偷挡住了。所以又写了俩个机器人，一个叫阶梯机器人，一个叫雷达机器人，把他们的传感器都放在了低位置，这次就还行了。

stairs_sensor_lidar.xacro

这样摆放老是让激光雷达把后面俩个方块当成障碍物，所以还是把后面俩个东西去掉了好扫描一点

stairs_sensors.xacro

两个模型的名字取反了，别弄混

新的代码和演示视频：百度网盘
提取码：t9nn

后续再写个ppt

十、总结