本文通过urdf和xacro两种方法在rviz里创建机器人模型!
一、创建机器人功能包
通过catkin_create_pkg 命令创建
catkin_create_pkg mrobot_description urdf xacro
二、在mrobot_description包下创建子目录
在该文件里需要创建的文件夹分别有config、launch、urdf。
config这个文件里放的是生成的urdf模型文件。
launch这个文件夹里放的是启动文件。
urdf这个文件夹里放的是机器人的urdf文件。
三、创建机器人urdf模型
在这里要注意的是xml文件里的首行一定不能是注释!!!!!!!!
<?xml version="1.0" ?>
<robot name="mbot">
<!--声明该文件使用XML描述,-->
<!--使用<robot>根标签定义一个机器人模型,模型的名称是“mbot”。-->
<!--描述机器人的地盘-->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.16" radius="0.20"/>
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
</link>
<!--机器人地盘与左轮的关节-->
<joint name="left_wheel_joint" type="continuous">
<origin xyz="0 0.19 -0.05" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--描述机器人的左轮-->
<link name="left_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.9"/>
</material>
</visual>
</link>
<!--机器人底盘与右轮的关节-->
<joint name="right_wheel_joint" type="continuous">
<origin xyz="0 -0.19 -0.05" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--描述机器人的的右轮-->
<link name="right_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0" />
<geometry>
<cylinder radius="0.06" length = "0.025"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.9"/>
</material>
</visual>
</link>
<!--机器人底盘与前支撑轮(作为万向轮)的关节-->
<joint name="front_caster_joint" type="continuous">
<origin xyz="0.18 0 -0.095" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="front_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--描述机器人前支撑轮(也作为万向轮)-->
<link name="front_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.015" />
</geometry>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
</visual>
</link>
<!--机器人底盘与后支撑轮(作为万向轮)的关节-->
<joint name="back_caster_joint" type="continuous">
<origin xyz="-0.18 0 -0.095" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="back_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--描述机器人后支撑轮(也作为万向轮)-->
<link name="back_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.015" />
</geometry>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
</visual>
</link>
</robot>
每个标签的含义为
<link>
<inertial> #惯性标签
<origin> #相对link的坐标
<mass> #质量
<inertial> #转动惯量
<visual> #视觉属性
<origin> #相对link标签
<geometry> #形状
<material> #材质
<collision> #碰撞属性
<origin> #相对坐标
<geometry> #形状
</link>
<joint><origin> #从父link到子link的变换
<parent> #父link
<child> #子link
<axis> #关节轴
<calibration> #视觉属性
<dynamics> #动力学参数
<limit> #关节限位
</joint>
像这里的惯性标签和碰撞标签在编写gazebo文件时才会使用。
四、编写launch文件
<launch>
<param name="robot_description" textfile="$(find mbot_description)/urdf/urdf/mbot_base.urdf" />
<!-- 设置GUI参数,显示关节控制插件 -->
<param name = "use_gui" value = "true"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!--这是用来发布机器人位置信息的节点-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>
launch文件里的内容大部分都是不变的,我们需要改的只有texefile的内容,里面的内容指向放置urdf文件的位置,书写格式为
"$(find 包名)/子目录/.../文件名"
五、运行结果
在编写完urdf文件之后,我们发现使用urdf编写的语言过于繁杂,那么下面我们来介绍另一种编写机器人描述文件的方法————使用xacro文件编写。
六、使用xacro编写机器人描述模型(mbot_base.xacro)
这里涉及一些关于xacro的语法,我在下篇文章详细描述!
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--声明该文件使用XML描述,-->
<!--使用xacro时必须要在开头添加上:xmlns:xacro="http://www.ros.org/wiki/xacro"-->
<!--参数的定义-->
<xacro:property name="M_PI" value="3.1415926"/>
<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"/>
<!--定义机器人所使用的颜色-->
<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>
<!-- 车轮的宏定义 相当于C语言中自己定义的函数 -->
<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>
</link>
</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>
</link>
</xacro:macro>
<!--小车底盘的宏定义-->
<!--base_link与base_footprint的区别-->
<!--base_link是固定在机器人本体上的坐标系,通常选择机器人腰部。-->
<!--base_footprint表示机器人base_link原点在地面上的投影,区别base_link之处是其“z”坐标不同。-->
<!--一般为了模型不陷入地面,base_footprint的“z”坐标比base_link高。-->
<xacro:macro name="mbot_base">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1 0.001 0.001" />
</geometry>
</visual>
</link>
<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>
</link>
<!--宏定义的使用 注意使用时前面一定要加xacro(noetic版本以前的不用加)-->
<xacro:wheel prefix="left" reflect="-1"/>
<xacro:wheel prefix="right" reflect="1"/>
<xacro:caster prefix="front" reflect="-1"/>
<xacro:caster prefix="back" reflect="1"/>
</xacro:macro>
</robot>
七、编写xacro机器人描述文件的引用文件(mbot.xacro)
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" />
<xacro:mbot_base/>
</robot>
八、编写launch文件
<launch>
<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot.xacro'" />
<arg name="gui" default="true" />
<param name="robot_description" command="$(arg model)" />
<!-- 设置GUI参数,显示关节控制插件 -->
<param name="use_gui" value="$(arg gui)"/>
<!-- 运行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 mbot_description)/config/mbot.rviz" required="true" />
</launch>
这里我们需要改的是default里面的东西,内容为xacro文件的地址,格式为
"$(find xacro)/xacro --inorder '$(find 包名)/子目录/..../文件名
这里的运行结果和urdf文件编写的模型运行结果一模一样。
总结
本文通过urdf和xacro两种描述方法编写了一个机器人模型。这里虽然还没从代码量上体现出xacro的优越性,但是xacro在大工程上面的优点会显而易见。因为xacro有定义函数的功能(宏定义)、计算的功能等等,可以让我们减少很多重复性工作。