week32 车体与环境仿真 navigation导航

地图

通过gazebo，搭建自己想要的地图。我一共搭建了三幅，分别应用于不同的场景。第一副是足球场，模拟户外环境。第二幅是家居环境。第三幅地图稍大，类似于迷宫。

ground
home
labyrinth

机器人

我仿真了两款机器人。第一款是圆形底盘的二轮差分机器人，白色轮子是主动轮，绿色轮子是万向轮，上面放置了hokuyo激光雷达。第二款是官方提供的turtlebot机器人，配有kinect深度相机和LDS激光雷达。

myrobot
turtlebot

导航

在此任务中，语音导航指令为主要任务。 使用SLAM预先构建的地图，机器人可以通过语音导航。 让我们想象一个炎热和你的情况‘ 我们坐在你的客厅里想要一杯啤酒，啤酒在没有空调的储藏室里。 所以，在这个时候，你可以要求机器人从仓库里给你买一瓶啤酒，只要说“储存”，它就会去往storage储藏室。 当你说“卧室”的工作方式相同时，机器人会自动进入卧室。 语音导航的基础是智能语音交互，使机器人能够理解你说了什么并采取了行动。 用move_base包移动机器人，我们可以使用actionlib将机器人引导到特定的目标位置。

start2storage
storage2bedroom
rqt_tf_tree
image.png
image.png
image.png

下面是车体仿真的源代码

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

<!-- property list -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="base_mass" value="1"/>
    <xacro:property name="base_length" value="0.20"/>
    <xacro:property name="base_radius" value="0.30"/>

    <xacro:property name="wheel_mass" value="0.2"/>
    <xacro:property name="wheel_length" value="0.05"/>
    <xacro:property name="wheel_radius" value="0.08"/>
    <xacro:property name="wheel_joint_y" value="0.28"/>
    <xacro:property name="wheel_joint_z" value="0.10"/>

    <xacro:property name="caster_mass" value="0.1"/>
    <xacro:property name="caster_radius" value="0.04"/>
    <xacro:property name="caster_joint_x" value="0.25"/>
    <xacro:property name="caster_joint_z" value="0.14"/>

<!-- define color -->
    <material name="white">
    <color rgba="1 1 1 0.95"/>
    </material>
    <material name="black">
    <color rgba="0 0 0 1"/>
    </material>
    <material name="blue">
    <color rgba="0 0 1 1"/>
    </material>
    <material name="red">
    <color rgba="1 0 0 1"/>
    </material>
    <material name="green">
    <color rgba="0 1 0 1"/>
    </material>
    <material name="yellow">
    <color rgba="1 0.4 0 1"/>
    </material>
    <material name="coffee">
    <color rgba="0.8 0.7 0.55 1"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>

<!-- macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
    <inertial>
        <mass value="${m}"/>
        <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
        iyy="${2*m*r*r/5}" iyz="0"
        izz="${2*m*r*r/5}" />
    </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
    <inertial>
        <mass value="${m}"/>
        <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
        iyy="${m*(3*r*r+h*h)/12}" iyz="0"
        izz="${m*r*r/2}" />
    </inertial>
    </xacro:macro>

<!-- macro for robot wheel -->
    <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 length="${wheel_length}" radius="${wheel_radius}"/>
        </geometry>
        <material name="gray"/>
        </visual>
        <collision>
        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
        <geometry>
            <cylinder length="${wheel_length}" radius="${wheel_radius}"/>
        </geometry>
        </collision>
        <cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}"/>
    </link>

    <gazebo reference="${prefix}_wheel_link">
        <material> Gazebo/Gray </material>
    </gazebo>

    <transmission name="${prefix}_wheel_joint_trans">
        <type> transmission_interface/SimpleTransmission </type>
        <joint name="${prefix}_wheel_joint">
        <hardwareInterface> hardware_interface/VelocityJointInterFace </hardwareInterface>
        </joint>
        <acutator name="${prefix}_wheel_joint_motor">
        <hardwareInterface> hardware_interface/VelocityJointInterFace </hardwareInterface>
        </acutator>
    </transmission>
    </xacro:macro>

<!-- macro for robot caster -->
    <xacro:macro name="caster" params="prefix reflect">
    <joint name="${prefix}_caster_joint" type="continuous">
        <origin xyz="${reflect*caster_joint_x} 0 ${-caster_joint_z}" 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="green"/>
        </visual>
        <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <sphere radius="${caster_radius}"/>
        </geometry>
        <material name="green"/>
        </collision>
        <sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}"/>
    </link>

    <gazebo reference="${prefix}_caster_link">
        <material> Gazebo/Green </material>
    </gazebo>
    </xacro:macro>

<!-- main function -->
    <xacro:macro name="myrobot_base_gazebo">
    <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>
    <gazebo reference="base_footprint">
        <turnGravityOff> false </turnGravityOff>
    </gazebo>
    <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 ${base_length/2+wheel_radius}" 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>
        <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
            <cylinder length="${base_length}" radius="${base_radius}"/>
            </geometry>
        </collision>
        <cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}"/>
    </link>

    <gazebo reference="base_link">
        <material> Gazebo/Blue </material>
    </gazebo>

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

    <!-- controller -->
    <gazebo>
        <plugin name="differential_drive_controller"
            filename="libgazebo_ros_diff_drive.so">
        <rosDebugLevel> Debug </rosDebugLevel>
        <publishWheelTF> true </publishWheelTF>
        <robotNamespace> / </robotNamespace>
        <publishTf> 1 </publishTf>
        <publishWheelJointState> true </publishWheelJointState>
        <alwaysOn> true </alwaysOn>
        <updateRate> 100 </updateRate>
        <legacyMode> true </legacyMode>
        <leftJoint> left_wheel_joint </leftJoint>
        <rightJoint> right_wheel_joint </rightJoint>
        <wheelSeparation> ${wheel_joint_y*2} </wheelSeparation>
        <wheelDiameter> ${wheel_radius*2} </wheelDiameter>
        <broadcastTF> 1 </broadcastTF>
        <wheelTorque> 30 </wheelTorque>
        <wheelAcceleration> 1.8 </wheelAcceleration>
        <commandTopic> cmd_vel </commandTopic>
        <odometryFrame> odom </odometryFrame>
        <odometryTopic> odom </odometryTopic>
        <robotBaseFrame> base_footprint </robotBaseFrame>
        </plugin>
    </gazebo>
    </xacro:macro>

</robot>

下面是雷达仿真的源代码

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

    <xacro:macro name="hokuyo" params="prefix:=laser">
        <!-- reference frame -->
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1"/>
                <origin xyz="0 0 0"/>
                <inertia ixx="0.01" ixy="0" ixz="0"
                     iyy="0.01" iyz="0"
                     izz="0.01"/>
            </inertial>

            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
                <material name="white"/>
            </visual>

            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <cylinder length="0.06" radius="0.05"/>
                </geometry>
            </collision>
        </link>

        <gazebo reference="${prefix}_link">
            <material> Gazebo/White </material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <sensor type="ray" name="head_hokuyo_sensor">
                <pose> 0 0 0 0 0 0 </pose>
                <visualize> false </visualize>
                <update_rate> 40 </update_rate>
                <ray>
                    <scan>
                        <horizontal>
                            <samples> 720 </samples>
                            <resolution> 1 </resolution>
                            <min_angle> -3.14 </min_angle>
                            <max_angle> 3.14 </max_angle>
                        </horizontal>
                    </scan>
                    <range>
                        <min> 0.1 </min>
                        <max> 30 </max>
                        <resolution> 0.01 </resolution>
                    </range>
                    <noise>
                        <type> gaussian </type>
                        <mean> 0 </mean>
                        <stddev> 0.01 </stddev>
                    </noise>
                </ray>
                <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
                    <topicName> /scan </topicName>
                    <frameName> laser_link </frameName>
                </plugin>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>