ROS机器人学习(二)--URDF模型优化

在官方教程中有一句话形容xacro的优势:

“Fortunately, you can use the xacro package to make your life simpler.”

XACRO格式提供了一些更高级的方式来组织编辑机器人描述.。可以通过宏定义，文件包含来精简模型文件。还可以通过定义常量、变量等来反复调用。

1、在urdf文件夹目录下新建mbot_base.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.1415926"/>	
    <xacro:property name="base_radius" value="0.16"/>
    <xacro:property name="base_length" value="0.005"/>

    <xacro:property name="wheel_radius" value="0.032"/>
    <xacro:property name="wheel_length" value="0.025"/>
    <xacro:property name="wheel_joint_x" value="0.09"/>
    <xacro:property name="wheel_joint_y" value="0.12"/>
    <xacro:property name="wheel_joint_z" value="0.01"/>

    <xacro:property name="caster_radius" value="0.022"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
    <xacro:property name="caster_joint_x" value="0.12"/>

     <!-- 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 0.9" />
	</material>

	<!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="${wheel_joint_x} ${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="mbot_base">
		<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>
        <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>
		
		<link name="back_caster_link">
			<visual>
				<origin xyz="0 0 0" rpy="0 0 0" />
				<geometry>
					<sphere radius="0.022" />
				</geometry>
				<material name="black">
					<color rgba="0 0 0 0.95" />
				</material>
			</visual>
		</link>
		<joint name="back_caster_joint" type="continuous">
			<origin xyz="-0.12 0 -0.018" rpy="0 0 0" />
			<parent link="base_link" />
			<child link="back_caster_link" />
			<axis xyz="0 1 0" />
		</joint>

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

</robot>

1.1）常量定义

<xacro:property name="M_PI" value="3.141592"/>

常量使用：

<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />

 1.2）数学计算

<origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />

1.3）宏定义 

<xacro:macro name="wheel" params="prefix reflect">
…………
</xacro:macro>

1.4）宏调用

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

2、继续新建一个lidar.xacro文件。这里把激光雷达模型单独拿出来成一个文件，方便灵活管理模型。

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

	<xacro:macro name="rplidar" params="prefix:=laser">
		<link name="${prefix}_link">
			<visual>
				<origin xyz=" 0 0 0 " rpy="0 0 0" />
				<geometry>
					<cylinder length="0.05" radius="0.05"/>
				</geometry>
				<material name="black"/>
			</visual>
		</link>
	</xacro:macro>

</robot>

3、继续新建一个mbot_with_laser.xacro文件，进行调用上面的mbot_base.xacro和lidar.xacro文件。

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" />
    <xacro:include filename="$(find mbot_description)/urdf/xacro/lidar.xacro" />
	
    <xacro:property name="laser_offset_x" value="0.08" />
    <xacro:property name="laser_offset_y" value="0" />
    <xacro:property name="laser_offset_z" value="0.03" />

    <mbot_base/>

    <!-- Laser -->
    <joint name="laser_joint" type="fixed">
        <origin xyz="${laser_offset_x} ${laser_offset_y} ${laser_offset_z}" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>

    <xacro:rplidar prefix="laser"/>

</robot>

4、在launch文件夹下新建display_mbot_with_laser_xacro.launch启动文件。

<launch>
	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/mbot_with_laser.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>

4、运行此launch文件，可以看到显示出来的机器人模型

roslaunch mbot_description display_mbot_with_laser_xacro.launch