gazebo创建机器人模型02

gazebo创建机器人模型02

1.实现流程

1.编写封装惯性矩阵算法的 xacro 文件
2.为机器人模型的每一个 link 添加 collision 和 inertial 标签，并且重置颜色属性
3.在launch文件中启动 gazebo 并添加机器人模型

1.1编写封装惯性矩阵的 xacro 文件

直接粘贴过来调用即可

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 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>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</robot>

1.2 机器人模型

将之前写过的 car.urdf.xacro、dem05_car_base_urdf.xacro、demo06_car_camera.urdf.xacro、demo07_car_laser.urdf.xacro，直接复制到 urdf02_gazebo 下的 urdf 目录下。
小车文件被分成几个部分，其组织形式为：先编写底盘部分，形成一个xacro文件，再编写其他传感器，形成相应的xacro文件，最后编写一个xacro文件将上述文件集成进来。

car.urdf.xacro
	demo05_car_base_urdf.xacro
	demo06_car_camera.urdf.xacro
	demo07_car_laser.urdf.xacro

对上述文件进行修改，使其符合 gazebo 中的格式

1. car.urdf.xacro

该文件将底盘、传感器等进行组装。

<robot name="mycar" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 包含惯性矩阵文件 -->
    <xacro:include filename="head.xacro" />
    <!-- 包含底盘、摄像头、雷达的 xacro 文件 -->
    <xacro:include filename="demo05_car_base.urdf.xacro" />
    <xacro:include filename="demo06_car_camera.urdf.xacro" />
    <xacro:include filename="demo07_car_laser.urdf.xacro" />

    <!-- 集成运动控制 -->
    <xacro:include filename="gazebo/move.xacro" />
    <!-- 雷达 -->
    <!-- <xacro:include filename="gazebo/laser.xacro" /> -->
    <!-- 摄像头 -->
    <xacro:include filename="gazebo/camera.xacro" />

        <!-- 深度相机 -->
    <xacro:include filename="gazebo/kinect.xacro" />
</robot>

2. demo05_car_base.urdf.xacro

主要修改的部分为：
（1）base_link添加 collision 和 inertial 标签，collision 可直接复制 visual 中的 geometry 和 origin 标签。inertial 调用 1.1 中的宏定义传参 m r h 即可实现圆柱的惯性矩阵设置。
（2）base_link 设置颜色，gazebo material 标签。
驱动轮、支撑轮设置同理

<robot name="mycar" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:property name="footprint_radius" value="0.001" />
    <!-- 1.先添加 base_footprint -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${footprint_radius}" />
            </geometry>
        </visual>
    </link>

        <!-- 2.添加底盘 -->
    <!-- 
        底盘参数：
            1.形状：圆柱
            2.半径：0.1m
            3.高度：0.08m
            4.离地间距：0.015m
     -->
    <xacro:property name="base_radius" value="0.1" />
    <xacro:property name="base_length" value="0.08" />
    <xacro:property name="base_mass" value="2" />
    <xacro:property name="lidi" value="0.015" />
    <xacro:property name="base_joint_z" value="${base_length/2 + lidi}" />
    <!-- 2.1 link -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}" />
            </geometry>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <material name="base_link_color">
                <color rgba="1.0 0.5 0.2 0.5" />
            </material>

        </visual>
        <collision>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}" />
            </geometry>

            <origin xyz="0 0 0" rpy="0 0 0" />
        </collision>
        <!-- 调用惯性矩阵函数 -->
        <xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Yellow</material>
    </gazebo>

    <!-- 2.2 joint -->
    <joint name="link2footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link" />
        <!-- 关节在 z 上的设置 = 车体高度/2+离地间距 -->
        <origin xyz="0 0 ${base_joint_z}" rpy="0 0 0" />
    </joint>

        <!-- 3.添加驱动轮 -->
    <!-- 
        形状：圆柱
        半径：0.035m
        宽度：0.015m
     -->
     <!-- 属性封装 -->
     <xacro:property name="wheel_radius" value="0.0325" />
     <xacro:property name="wheel_length" value="0.015" />
     <xacro:property name="wheel_mass" value="0.05" />
     <xacro:property name="PI" value="3.1415927" />
     <!-- 注意：结果是负数 -->
     <xacro:property name="wheel_joint_z" value="${(base_length/2 + lidi - wheel_radius) * -1}" />

    <!-- wheel_name: left or right -->
    <!-- flag: 1 or -1 -->
    <xacro:macro name="wheel_func" params="wheel_name flag">

        <!-- 3.1 link -->
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

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

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>

            </visual>
            <collision>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI/2} 0 0" />
            </collision>
            <xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link>
        <gazebo reference="${wheel_name}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>  
    
        <!-- 3.2 joint -->
        <joint name="base_${wheel_name}_wheel_joint" type="continuous">
            <parent link="base_link" />
            <child link="${wheel_name}_wheel" />
            <!-- 
                x 0
                y 0.1
                z 0.0225
            -->
            <origin xyz="0 ${0.1 * flag} ${wheel_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>

    <xacro:wheel_func wheel_name="l" flag="1" />
    <xacro:wheel_func wheel_name="r" flag="-1" />

    <!-- 4.添加万向轮 -->
    <!-- 
        形状：球
        半径：0.0075
     -->
    <xacro:property name="small_wheel_radius" value="0.0075" />
    <xacro:property name="small_wheel_mass" value="0.01" />
    <!-- z的偏移量 = 车体高度/2+离地间距 - 万向轮半径 -->
    <xacro:property name="small_joint_z" value="${(base_length/2 + lidi - small_wheel_radius) * -1}" />

    <xacro:macro name="small_wheel_func" params="small_wheel_name flag " >
        <!-- 4.1 设置 link -->
        <link name="${small_wheel_name}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${small_wheel_radius}" />
                </geometry>

                <origin xyz="0 0 0" rpy="0 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>

            </visual>
            <collision>
                <geometry>
                    <sphere radius="${small_wheel_radius}" />
                </geometry>

                <origin xyz="0 0 0" rpy="0 0 0" />
            </collision>
            <xacro:sphere_inertial_matrix m="${small_wheel_mass}" r="${small_wheel_radius}" />
        </link>  
        <gazebo reference="${small_wheel_name}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>

        <!-- 4.2 设置 joint -->
        <joint name="${small_wheel_name}2link" type="continuous">
            <parent link="base_link" />
            <child link="${small_wheel_name}_wheel" />
            <!-- 
                x 0.08
                y 0.1
                z 0.0225
            -->
            <origin xyz="${0.08 * flag} 0 ${small_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>
    
    </xacro:macro>  

    <xacro:small_wheel_func small_wheel_name="front" flag="1" />
    <xacro:small_wheel_func small_wheel_name="back" flag="-1" />

</robot>

3. demo06_car_camera.urdf.xacro

collision 添加：复制 visual 标签中的 geometry 即可，origin 没有的话就不用管了。
惯性矩阵调用第一步的封装即可。

<robot name="mycar" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 摄像头部件 -->
    <!-- 1.参数 -->
    <!-- 
        参数：
            连杆属性：厚度、宽度、高度
            关节属性：x y z
     -->
    <xacro:property name="camera_length" value="0.02" />     <!-- 厚度（x）-->    
    <xacro:property name="camera_width" value="0.05" />     <!-- 宽度（y）-->  
    <xacro:property name="camera_height" value="0.04" />     <!-- 高度（z）-->
    <xacro:property name="camera_mass" value="0.01" />     <!-- 高度（z）-->
 

    <xacro:property name="joint_camera_x" value="0.08" />     <!-- -->  
    <xacro:property name="joint_camera_y" value="0" />     <!-- -->  
    <xacro:property name="joint_camera_z" value="${base_length/2 + camera_height/2}" />     <!-- -->  
    <!-- 2.连杆和关节 -->

    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>

            <material name="black">
                <color rgba="0 0 0 0.8" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
        <xacro:Box_inertial_matrix m="${camera_mass}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
        </collision>
    </link>
    <gazebo reference="camera">
        <material>Gazebo/Blue</material>
    </gazebo>

    <joint name="camera2base" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${joint_camera_x} ${joint_camera_y} ${joint_camera_z}" rpy="0 0 0" />
    </joint>

</robot>

4. demo07_car_laser.urdf.xacro

支架、雷达：
（1）复制 visual 中的实现到 collision 中
（2）添加惯性矩阵宏，使用 cylinder 传入 m r h 三个参数
（3）设置颜色 Gazebo/Black 等

<robot name="mycar" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 雷达部件 -->

    <!-- 参数 -->
    <!-- 
        1.支架
            尺寸：半径 高度
            关节偏移量
        2.雷达
            尺寸：半径 高度
            关节偏移量
     -->
    <xacro:property name="support_radius" value="0.01" />
    <xacro:property name="support_length" value="0.15" />
    <xacro:property name="support_mass" value="0.1" />

    <xacro:property name="laser_radius" value="0.03" />
    <xacro:property name="laser_length" value="0.05" />
    <xacro:property name="laser_mass" value="0.15" />

    <xacro:property name="joint_support_x" value="0" />
    <xacro:property name="joint_support_y" value="0" />
    
    <!-- z = 车体高度/2 + 支架高度/2 -->
    <xacro:property name="joint_support_z" value="${base_length/2 + support_length/2}" />
    
    <xacro:property name="joint_laser_x" value="0" />
    <xacro:property name="joint_laser_y" value="0" />

    <!-- z = 支架高度/2 + 雷达高度/2 -->
    <xacro:property name="joint_laser_z" value="${support_length/2 + laser_length/2}" />

    <!-- 1. 雷达支架 -->
    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>

            <material name="yellow">
                <color rgba="0.8 0.5 0.0 0.5" />
            </material>

        </visual>
        
        <collision>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry> 
        </collision>
        <xacro:cylinder_inertial_matrix m="${support_mass}" r="${support_radius}" h="${support_length}" />
    </link>
    <gazebo reference="support">
        <material>Gazebo/Gray</material>
    </gazebo>

    <joint name="support2base" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${joint_support_x} ${joint_support_y} ${joint_support_z}" rpy="0 0 0" />
    </joint>
    <!-- 2.雷达 -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>

            <material name="black">
                <color rgba="0 0 0 0.5" />
            </material>

        </visual>
        
        <collision>
            <geometry>
                    <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
        </collision>
        <xacro:cylinder_inertial_matrix m="${laser_mass}" r="${laser_radius}" h="${laser_length}" />

    </link>

    <gazebo reference="laser">
        <material>Gazebo/Black</material>
    </gazebo>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${joint_laser_x} ${joint_laser_y} ${joint_laser_z}" rpy="0 0 0" />
    </joint>


</robot>

1.3编写 launch 启动文件

demo02_car.launch

<launch>
    <!-- 1. 需要在参数服务器中载入 urdf -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
    <!-- 2. 需要启动 gazebo 仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    <!-- 3. 在gazebo 中添加机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />
</launch>

2.Gazebo 仿真环境集成

实现流程：
（1）在功能包下新建一个文件夹，命名为worlds，将box_house.world 复制到该文件夹下。
（2）新建一个 launch 文件，写入如下内容：可以复制1.3中的启动文件内容，然后对启动 gazebo 仿真环境部分进行修改，1.3中加载了一个空的世界环境，本小节在此基础上添加一个参数 arg ，其参数 name = "world_name"为固定的。

demo03_env.launch

<arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />

<launch>
    <!-- 1. 需要在参数服务器中载入 urdf -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
    <!-- 2. 需要启动 gazebo 仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 3. 在gazebo 中添加机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />
</launch>