（十四）构建、仿真移动机器人模型

模型文件

创建程序包mobot_urdf，需要的依赖项是roscpp和std_msgs，这次使用的模型是xacro类型的文件，据说是可以简化使用的urdf模型，可以把一个整体拆分为多个部分，分别在各个文件中实现。
主文件：

<?xml version="1.0"?>
<robot 
     xmlns:xacro="http://www.ros.org/wiki/xacro" name="mobot">
    <xacro:include filename="$(find mobot_urdf)/urdf/mobot_xacro_defs.xacro" />
    <xacro:include filename="$(find mobot_urdf)/urdf/mobot_static_links.xacro" />
    <xacro:include filename="$(find mobot_urdf)/urdf/mobot_wheels.xacro" />    
    <xacro:include filename="$(find mobot_urdf)/urdf/casters.xacro" />
    <xacro:include filename="$(find mobot_urdf)/urdf/gazebo_tags.xacro" />
</robot>

子文件：

<?xml version="1.0"?>
<robot 
     xmlns:xacro="http://www.ros.org/wiki/xacro" >
     <!-- define the base-link origin to lie at floor level, between the drive wheels-->
     <!--main body is a simple box; origin is a center of box-->
    <xacro:property name="bodylen" value="0.5461" />
    <xacro:property name="bodywidth" value="0.4572" />
    <xacro:property name="bodyheight" value="0.2" />
    <xacro:property name="bodyclearance" value="0.4" />  <!--clearance from bottom of box to ground-->  
    <!-- derived values-->
    <xacro:property name="half_bodylen" value="${bodylen/2.0}" /> 
    <xacro:property name="half_bodyheight" value="${bodyheight/2.0}" /> 
    <!-- placement of main body relative to base link frame -->
    <xacro:property name="bodyOX" value="${-half_bodylen}" />  
    <xacro:property name="bodyOY" value="0" />   
    <xacro:property name="bodyOZ" value="0.45" />     
            
    <!-- define the drive-wheel dimensions-->
    <xacro:property name="tirediam" value="0.3302" />
    <xacro:property name="tirerad" value="${tirediam/2.0}" />    
    <xacro:property name="tirewidth" value="0.06985" />
    <!-- "track" is the distance between the drive wheels -->
    <xacro:property name="track" value=".56515" />
    
    <!-- battery box dimensions -->
    <xacro:property name="batterylen" value="0.381" />
    <xacro:property name="batterywidth" value="0.3556" />
    <xacro:property name="batteryheight" value="0.254" />
    <!-- placement of battery box relative to base frame -->
    <xacro:property name="batOX" value="-0.05" />  
    <xacro:property name="batOY" value="0" />   
    <xacro:property name="batOZ" value="0.22" />   


    <xacro:property name="M_PI" value="3.1415926535897931" />
    <xacro:property name="boschwidth" value="0.0381" />
    <xacro:property name="casterdrop" value="0.125" />
    <xacro:property name="bracketwidth" value="0.1175" />
    <xacro:property name="bracketheight" value="0.16" />
    <xacro:property name="bracketthick" value="0.0508" />
    <xacro:property name="bracketangle" value="0.7854" />
    <xacro:property name="casterwidth" value="0.0826" />
    <xacro:property name="casterdiam" value="0.2286" />
    
    <!--here is a default inertia matrix with small, but legal values; use this when don't need accuracy for I -->
    <!--model will assign inertia matrix dominated by main body box -->
    <xacro:macro name="default_inertial" params="mass">
        <inertial>
            <mass value="${mass}" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0"
         iyy="0.01" iyz="0.0"
         izz="0.01" />
        </inertial>
    </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot 
     xmlns:xacro="http://www.ros.org/wiki/xacro" name="static_links">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="${bodylen} ${bodywidth} ${bodyheight}"/>
            </geometry>
            <origin xyz="${bodyOX} ${bodyOY} ${bodyOZ}" rpy="0 0 0"/>
        </visual>
        <collision>
            <geometry>
                <box size="${bodylen} ${bodywidth} ${bodyheight}"/>
            </geometry>
            <origin xyz="${bodyOX} ${bodyOY} ${bodyOZ}" rpy="0 0 0"/>
        </collision>
        <inertial>
            <!--assign almost all the mass to the main body box; set m= 100kg; treat I as approx m*r^2 -->
            <mass value="100" />
            <inertia ixx="10" ixy="0" ixz="0"
              iyy="10" iyz="0"
              izz="10" />
        </inertial>
    </link>

    <link name="batterybox">
        <visual>
            <geometry>
                <box size="${batterylen} ${batterywidth} ${batteryheight}"/>
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </visual>
        <collision>
            <geometry>
                <box size="${batterylen} ${batterywidth} ${batteryheight}"/>
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>
    <joint name="batterytobase" type="fixed">
        <parent link="base_link"/>
        <child link="batterybox"/>
        <origin xyz="${batOX} ${batOY} ${batOZ}" rpy="0 0 0"/>
    </joint>

        

</robot>

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

    <xacro:macro name="wheel" params="prefix reflect">
        <link name="${prefix}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${tirerad}" length="${tirewidth}"/>
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <cylinder radius="${tirerad}" length="${tirewidth}"/>
                </geometry>
            </collision>
            <inertial>
            <!--assign inertial properties to drive wheels -->
            <mass value="1" />
            <inertia ixx="0.1" ixy="0" ixz="0"
              iyy="0.1" iyz="0"
              izz="0.1" />
        </inertial>
        </link>
        <joint name="${prefix}_wheel_joint" type="continuous">
            <axis xyz="0 0 1"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel"/>
            <origin xyz="0 ${reflect*track/2} ${tirerad}" rpy="0 ${M_PI/2} ${M_PI/2}"/>
            <limit effort="100" velocity="15" />
            <joint_properties damping="0.0" friction="0.0" />
        </joint>
    </xacro:macro>
    <xacro:wheel prefix="left" reflect="1"/>
    <xacro:wheel prefix="right" reflect="-1"/>
</robot>

<?xml version="1.0"?>
<robot 
     xmlns:xacro="http://www.ros.org/wiki/xacro" name="casters">
 
    <xacro:macro name="caster" params="prefix reflect">
        <link name="castdrop_${prefix}">
            <visual>
                <geometry>
                    <box size="${boschwidth} ${boschwidth} ${casterdrop}"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
            </visual>
            <collision>
                <geometry>
                    <box size="${boschwidth} ${boschwidth} ${casterdrop}"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
            </collision>
            <xacro:default_inertial mass="0.2"/>
        </link>
        <joint name="cast2base_${prefix}" type="fixed">
            <parent link="base_link"/>
            <child link="castdrop_${prefix}"/>
            <origin xyz="${-bodylen/2+bodyOX+boschwidth/2} ${reflect*bodywidth/2-reflect*boschwidth/2} ${-casterdrop/2-bodyheight/2+bodyOZ}" />
        </joint>
        <link name="brackettop_${prefix}">
            <visual>
                <geometry>
                    <box size="${bracketwidth} ${bracketthick} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
            </visual>
            <collision>
                <geometry>
                    <box size="${bracketwidth} ${bracketthick} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
            </collision>
            <xacro:default_inertial mass="0.2"/>
        </link>
        <joint name="cast2bracket_${prefix}" type="continuous">
            <axis xyz="0 0 1"/>
            <parent link="castdrop_${prefix}"/>
            <child link="brackettop_${prefix}"/>
            <origin xyz="0 0 ${-casterdrop/2}" rpy="0 0 ${M_PI/2}"/>
            <joint_properties damping="0.0" friction="0.0" />
        </joint>
        <link name="bracketside1_${prefix}">
            <visual>
                <geometry>
                    <box size="${bracketthick} ${bracketheight} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="${M_PI/2} ${-bracketangle} ${M_PI/2}"/>
            </visual>
            <collision>
                <geometry>
                    <box size="${bracketthick} ${bracketheight} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="${M_PI/2} ${-bracketangle} ${M_PI/2}"/>
            </collision>
            <xacro:default_inertial mass="0.2"/>
        </link>
        <joint name="brack2top1_${prefix}" type="fixed">
            <parent link="brackettop_${prefix}"/>
            <child link="bracketside1_${prefix}"/>
            <origin xyz="${bracketwidth/2} .04 -${bracketheight/2-.02}" rpy="0 0 0" />
        </joint>
        <link name="bracketside2_${prefix}">
            <visual>
                <geometry>
                    <box size="${bracketthick} ${bracketheight} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="${M_PI/2} ${-bracketangle} ${M_PI/2}"/>
            </visual>
            <collision>
                <geometry>
                    <box size="${bracketthick} ${bracketheight} .005"/>
                </geometry>
                <origin xyz="0 0 0" rpy="${M_PI/2} ${-bracketangle} ${M_PI/2}"/>
            </collision>
            <xacro:default_inertial mass="0.2"/>
        </link>
        <joint name="brack2top2_${prefix}" type="fixed">
            <parent link="brackettop_${prefix}"/>
            <child link="bracketside2_${prefix}"/>
            <origin xyz="${-bracketwidth/2} .04 -${bracketheight/2-.02}" rpy="0 0 0" />
        </joint>
        <link name="${prefix}_casterwheel">
            <visual>
                <geometry>
                    <cylinder radius="${casterdiam/2}" length="${casterwidth}"/>
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <cylinder radius="${casterdiam/2}" length="${casterwidth}"/>
                </geometry>
            </collision>
            <!-- accept default inertial properties for caster wheels-->
            <xacro:default_inertial mass="0.5"/>
        </link>
        <joint name="${prefix}_caster_joint" type="continuous">
            <axis xyz="0 0 1"/>
            <parent link="bracketside1_${prefix}"/>
            <child link="${prefix}_casterwheel"/>
            <origin xyz="${-casterwidth/2-.02} .053 -.053" rpy="0 ${M_PI/2} 0"/>
            <limit effort="100" velocity="15" />
            <joint_properties damping="0.0" friction="0.0" />
        </joint>
    </xacro:macro>
    <xacro:caster prefix="left" reflect="1"/>
    <xacro:caster prefix="right" reflect="-1"/>    
</robot>

<?xml version="1.0"?>
<robot 
     xmlns:xacro="http://www.ros.org/wiki/xacro" name="gazebo_tags">
       
 <gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>100</updateRate>
    <leftJoint>right_wheel_joint</leftJoint>
    <rightJoint>left_wheel_joint</rightJoint>
    <wheelSeparation>${track}</wheelSeparation>
    <wheelDiameter>${tirediam}</wheelDiameter>
    <torque>200</torque>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_link</robotBaseFrame>
    <publishWheelTF>true</publishWheelTF>
    <publishWheelJointState>true</publishWheelJointState>
  </plugin>
 </gazebo> 

</robot>

启动文件mobot.launch。

<launch>
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find mobot_urdf)/urdf/mobot.xacro'" />

<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model mobot" />
</launch>

若要使用xacro文件，可以使用xacro包中的xacro执行程序将它转化为urdf类型的模型文件。

rosrun xacro xacro mobot.xacro > mobot,urdf

执行之后，就可以看到一个新的urdf文件，这就是转化过后的模型文件了。

urdf_to_graphiz mobot.urdf

执行上面的命令，可以看到模型的架构。

加载空白世界。

roslaunch gazebo_ros empty_world.launch

加载机器人模型。

roslaunch mobot_urdf mobot.launch

这就是加载出来的机器人的效果。

移动机器人的控制

我们可以通过cmd_vel主题来控制，第一种方法是命令行，参见之前的方法，改一下主题就可以了；第二种方法是通过启动文件，这里也不过多的赘述了；下面我们玩一种比较新奇的控制方法，键盘控制（至少对我来说是很新奇，对大神来说就司空见惯了），直接运行一下命令就可以控制了。

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

我主要想说一下具体的控制方法，u是左前，i是前，o是右前，j是左旋转，k是静止，l是右旋转，m是左后，“ ，”是后，“ 。”是右后，具体的图就不贴了，还是很好玩的，大家一试便知。