前面 URDF 文件构建机器人模型的过程中,存在若干问题。
问题1:在设计关节的位置时,需要按照一定的公式计算,公式是固定的,但是在 URDF 中依赖于人工计算,存在不便,容易计算失误,且当某些参数发生改变时,还需要重新计算。
问题2:URDF 中的部分内容是高度重复的,驱动轮与支撑轮的设计实现,不同轮子只是部分参数不同,形状、颜色、翻转量都是一致的,在实际应用中,构建复杂的机器人模型时,更是易于出现高度重复的设计,按照一般的编程涉及到重复代码应该考虑封装。
在编程语言中,可以通过变量结合函数直接解决上述问题,在 ROS 中,已经给出了类似编程的优化方案,称之为:Xacro
初体验xacro
需求描述:
创建一个四轮圆柱状机器人模型,机器人参数如下,底盘为圆柱状,半径 10cm,高 8cm,四轮由两个驱动轮和两个万向支撑轮组成,两个驱动轮半径为 3.25cm,轮胎宽度1.5cm,两个万向轮为球状,半径 0.75cm,底盘离地间距为 1.5cm(与万向轮直径一致)
使用xacro优化上一节案例中驱动轮实现,需要使用变量封装底盘的半径、高度,使用数学公式动态计算底盘的关节点坐标,使用 Xacro 宏封装轮子重复的代码并调用宏创建两个轮子(注意: 在此,演示 Xacro 的基本使用,不必要生成合法的 URDF )。
在xacro下新建文件 demo01_helloworld.urdf.xacro
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 属性封装 -->
<xacro:property name="wheel_radius" value="0.0325" />
<xacro:property name="wheel_length" value="0.0015" />
<xacro:property name="PI" value="3.1415927" />
<xacro:property name="base_link_length" value="0.08" />
<xacro:property name="lidi_space" value="0.015" />
<!-- 宏 -->
<xacro:macro name="wheel_func" params="wheel_name flag" >
<link name="${wheel_name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<!-- 3-2.joint -->
<joint name="${wheel_name}2link" type="continuous">
<parent link="base_link" />
<child link="${wheel_name}_wheel" />
<!--
x 无偏移
y 车体半径
z z= 车体高度 / 2 + 离地间距 - 车轮半径
-->
<origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
<xacro:wheel_func wheel_name="left" flag="1" />
<xacro:wheel_func wheel_name="right" flag="-1" />
</robot>
第一个终端运行 roscore
第二个在xacro文件夹下 集成终端中打开
rosrun xacro xacro demo01_helloworld.urdf.xacro
显示出大段文本----将xacro文件解析成urdf文件
也可以将xacro文件导出为urdf文件
rosrun xacro xacro demo01_helloworld.urdf.xacro > demo01_helloworld.urdf
可见生成的文件与打印的文本相同
xacro语法详解
在xacro下新建文件 demo02_Field.urdf.xacro
属性
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 1.属性定义 -->
<xacro:property name="PI" value="3.1415927"/>
<xacro:property name="radius" value="0.03" />
<!-- 2.属性调用 -->
<!-- 3.算术运算 -->
</robot>
运行看看效果 rosrun xacro xacro demo02_Field.urdf.xacro
可见将注释保留 但定义属性没有显示。-定义后必须被调用
宏定义
类似于函数实现,提高代码复用率,优化代码结构,提高安全性
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<!--1.宏定义xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)"-->
<xacro:macro name="getSum" params="num1 num2">
<result value="${num1 + num2}"/>
</xacro:macro>
<!-- 2.宏调用 -->
<xacro:getSum num1="1" num2 ="5"/>
</robot>
运行看看效果 rosrun xacro xacro demo03_macro.urdf.xacro
文件包含
机器人由多部件组成,不同部件可能封装为单独的 xacro 文件,最后再将不同的文件集成,组合为完整机器人,可以使用文件包含实现
需求: 创建一个文件 包含之前demo02 03的相关文件
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 演示文件包含 -->
<!-- filename中为被包含的文件 -->
<xacro:include filename="demo02_Field.urdf.xacro" />
<xacro:include filename="demo03_macro.urdf.xacro" />
</robot>
运行看看效果 rosrun xacro xacro demo04_sun.urdf.xacro