【探索之路】机器人篇(3)-给mwRobot建立模型

　　在创建一个mwRobot_description程序包那一节中，我们添加了依赖roscpp  rospy std_msgs 和 urdf , 现在我们再添加一个xacro依赖。

　　如何添加依赖？

　　打开程序包中的package.xml文件，添加以下内容：

　　　　<build_depend>xacro</build_depend>

　　　　<build_export_depend>xacro</build_export_depend>

　　　　<exec_depend>xacro</exec_depend>

　　具体添加到何处请参考已经添加的roscpp依赖。

　　在程序包中新建以下文件夹：

　　　　urdf　　：存放机器人模型的URDF或xacro文件

　　　　meshes　：放置URDF中引用的模型渲染文件

　　　　launch　：保存相关启动文件

　　　　config  ：保存rviz的配置文件

　　　　mkdir urdf meshes launch config
　　在launch文件夹中添加 display_mwRobot_base_urdf.launch 文件

　　display_mwRobot_base_urdf.launch 文件内容如下:　

<!-- 利用urdf建立模型 -->
<launch>
    <param name="robot_description" textfile="$(find mwRobot_description)/urdf/mwRobot_base.urdf" />

    <!-- 显示关节控制插件，可以使关节回到中心位置也能设置关节为随机角度 -->
    <param name="use_gui" value="true" />

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
    <node name="joint_state_publisher_mwRobot" pkg="joint_state_publisher" type="joint_state_publisher" />

    <!-- 运行robot_state_publisher节点，将机器人各个links、joints之间的关系通过tf发布 -->
    <node name="mwRobot_state_publisher_mwRobot" pkg="robot_state_publisher" type="state_publisher" />

    <!-- 运行rviz可视化界面 -->
    <node name="mwRobot_rviz" pkg="rviz" type="rviz" args="-d $(find mwRobot_description)/config/mwRobot_urdf.rviz" required="true"/>

</launch>

　　这个launch文件的功能就是加载urdf模型文件，然后运行 joint_state_publisher 和 robot_state_publisher 这两个节点，它们是用来发布关节的状态信息和机器人各个links与joints之间的关系。所以这两个是必须要有的。最后一个就所运行rviz可视化工具来看我们机器人的模型。

　　由上述文件可得，我们目前还缺少 urdf/mwRobot_base.urdf 和 config/mwRobot_urdf.rviz文件。

创建urdf/mwRobot_base.urdf文件

　　在urdf文件夹中创建一个mwRobot_base.urdf文件。并输入以下内容:(在此文件中不能有中文注释，否则报错)

<?xml version="1.0" ?>
<robot name="mwRobot">

    <!--  No Chinese annotations exist.  -->

    <!-- Robot main body -->
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.16" radius="0.2"/>
            </geometry>
            <material name="yellow">
                <color rgba="1 0.4 0 1" />
            </material>

        </visual>
    </link>

    <!-- The connection between the left wheel and the main body -->
    <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>

    <!-- The definition of the left wheel -->
    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder length="0.06" radius="0.08"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9" />
            </material>
        </visual>
    </link>

    <!-- The connection between the right wheel and the main body -->
    <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>

    <!-- The definition of the right wheel -->
    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder length="0.06" radius="0.08"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9" />
            </material>
        </visual>
    </link>

    <!--  Joint of front universal wheel and main body -->
    <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>

    <!-- Definition of front universal wheel -->
    <link name="front_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <sphere radius="0.015"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.9" />
            </material>
        </visual>
    </link>

    <!--  Joint of back universal wheel and main body -->
    <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>

    <!-- Definition of back universal wheel -->
    <link name="back_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <sphere radius="0.015"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.9" />
            </material>
        </visual>
    </link>

</robot>

　　此文件中定义了机器人的主体、两个主动轮、两个从动论轮以及它们与主体的关节(连接)。

创建config/mwRobot_urdf.rviz 文件

　　此文件为rviz的配置文件，可以在rviz软件中随意修改，要求并不苛刻。所有参数的修改都可以通过RVIZ来完成。

　　首先，启动rviz。启动rviz之前需要启动roscore。所以打开两个控制终端，先在一个中启动roscore，然后在另一个启动rosrun rviz rviz。

　　　　roscore

　　　　rosrun rviz rviz

　　执行之后会打开一个软件，然后在左上角的files中点击Save Config As，然后保存到 config/mwRobot_urdf.rviz 即可。

　　

　　图：rviz软件启动界面

　　

查看我们创建的模型

　　我们把打开的软件关闭，关闭roscore。然后执行roslaunch 包名  launch文件名。

　　　　roslaunch mwRobot_description display_mwRobot_base_urdf.launch
　　然后程序会自动创建roscore(如果已经存在则不创建)，并打开rviz。

　　但是打开的RVIZ中并没有任何物体，这是因为我们还没有添加显示模型这个组件。

　　打开的RVIZ和创建RVIZ配置文件时的是一模一样。

　　这里我们先在左边Displays一栏的Global Options中，讲Fixed Frame修改为 base_link。

　　然后添加显示模型组件，在程序左下角有 Add Duplicate Remove Rename几个按钮，点击Add ，找到RobotModel选中并点击确定。这个时候就可以看到我们创建的模型了。

　　

　　再点击Add 选择TF并添加。就可以看到模型中有5个TF坐标显示。我们可以在另一个窗口Joint State Publisher 中来控制轮子的角度。

　　

　　至此，我们已经可以看到我们的模型了。我们可以将RVIZ此时配置保存起来，那么在下次打开的时候就和我们现在看到的一样，不用在手动去添加tf、robotmodel了。在rviz中按Ctrl+S即可完成保存。

改进URDF模型，使用xacro模型文件

　　在urdf文件中，我们发现有大量的代码重复，且参数修改麻烦，不便于二次开发，没有参数计算等功能。那么我们换成以一种形式 xacro。

　　在使用xacro模型文件中，我们可以使用xacro模型解析器，也可以生成udrf文件来使用。

　　我们先创建一个xacro文件 mwRobot_Mainpart.xacro文件。此文件将会包含机器人主体部分，不包含传感器等。

　　以下是mwRobot_Mainpart.xacro的内容:

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

    <!--  No Chinese annotations exist.  -->

    <!-- PROPERTY LIST -->
    <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"/>

    <!-- 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="wheel" params="prefix reflect">
        <!-- The connection between the wheel and the main body -->
        <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>

        <!-- The definition of the wheel -->
        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder length="${wheel_radius}" radius="${wheel_length}"/>
                </geometry>
                <material name="gray"/>
            </visual>
        </link>
    </xacro:macro>

    <xacro:macro name="caster" params="prefix reflect">
        <!--  Joint of universal wheel and main body -->
        <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>

        <!-- Definition of universal wheel -->
        <link name="${prefix}_caster_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <sphere radius="${caster_radius}"/>
                </geometry>
                <material name="black"/>
            </visual>
        </link>
    </xacro:macro>

    <!-- Robot main body -->
    <xacro:macro name="mwRobot_MainPart">

        <!--  The joints between robots and his shadow  -->
        <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>

        <!--  Projection of robot body on the ground  -->
        <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>

        <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>

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

        <caster prefix="front" reflect="-1"/>
        <caster prefix="back" reflect="1"/>

    </xacro:macro>

</robot>

　上面文件中，定义了机器人的地盘结构，但是我们的机器人也有可能添加双目相机、激光雷达等传感器，这些传感器大多分开写成一个xacro文件，所以我们还需要一个文件把一个机器人的各个模块组合起来。这里我新建一个 mwRobot_CompleteModel.xacro文件来处理这个事儿。

　　mwRobot_CompleteModel.xacro文件内容如下:

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

    <xacro:include filename="$(find mwRobot_description)/urdf/mwRobot_Mainpart.xacro" />

    <mwRobot_MainPart/>

</robot>

　　下面，我们就要修改launch文件来启动加载我们的模型文件了。这里我用的是xacro模型解析器。

　　复制原来的launch文件，并重命名为 display_mwRobot_CompleteModel.launch ，然后修改其中的代码：

<!-- 利用urdf建立模型 -->
<launch>
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find mwRobot_description)/urdf/mwRobot_CompleteModel.xacro'"/>
    <param name="robot_description" command="$(arg model)" />

    <!-- 显示关节控制插件，可以使关节回到中心位置也能设置关节为随机角度 -->
    <param name="use_gui" value="true" />

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
    <node name="joint_state_publisher_mwRobot" pkg="joint_state_publisher" type="joint_state_publisher" />

    <!-- 运行robot_state_publisher节点，将机器人各个links、joints之间的关系通过tf发布 -->
    <node name="mwRobot_state_publisher_mwRobot" pkg="robot_state_publisher" type="state_publisher" />

    <!-- 运行rviz可视化界面 -->
    <node name="mwRobot_rviz" pkg="rviz" type="rviz" args="-d $(find mwRobot_description)/config/mwRobot_urdf.rviz" required="true"/>

</launch>

　　保存，然后启动程序。

　　　　roslaunch mwRobot_description display_mwRobot_CompleteModel.launch
　　rviz启动后应该就可以看到模型了，和我们之前用urdf创建的是一样的。(有些误差，因为修改了两个轮子的参数)

　　如果不能看到模型先看看左侧的RVIZ设置是不是正确的。

　　下面是我打开的界面：

　　

　　这样，我们的一个机器人模型就创建好了，大家可以自行修改参数来设计一个自己喜欢的机器人。

　　本节工程已经上传，代号为V3.