URDF备份

<?xml version="1.0"?>
<robot name="rbo" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- variable -->
    <xacro:property name="PI" value="3.1415926"/>
    <xacro:property name="car_width" value="1.5"/>
    <xacro:property name="car_length" value="1.5"/>
    <xacro:property name="car_height" value="0.2"/>
    <xacro:property name="wheel_radius" value="0.2"/>
    <xacro:property name="wheel_origin_xyz" value="0.0 0.0 0.0"/>
    <xacro:property name="wheel_origin_rpy" value="0.0 0.0 0.0"/>


    <!-- macro -->
    <xacro:macro name="default_inertial" params="mass">
        <inertial>
            <mass value="${mass}" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="box_geometry" params="width length height">
        <geometry>
            <box size="${width} ${length} ${height}"/>
        </geometry>
    </xacro:macro>

    <xacro:macro name="sphere_geometry" params="radius">
        <geometry>
            <sphere radius="${radius}"/>
        </geometry>
    </xacro:macro>


    <!-- links -->
    <link name="car_link">
        <visual>
            <xacro:box_geometry width="${car_width}" length="${car_length}" height="${car_height}"/>
        </visual>
        <collision>
            <xacro:box_geometry width="${car_width}" length="${car_length}" height="${car_height}"/>
        </collision>
        <xacro:default_inertial mass="3.0"/>

    </link>

    <!-- gazebo -->
    <gazebo reference="car_link">
        <mu1>0.5</mu1>
        <mu2>0.5</mu2>
    </gazebo>

    <!-- wheel joint macro -->
    <!-- right:1 left:-1 -->
    <xacro:macro name="wheel_car_joint" params="wheel_name left_right">
        <link name="${wheel_name}">
            <visual>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </visual>
            <collision>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </collision>
            <xacro:default_inertial mass="1.0"/>
        </link>

        <!-- joints -->
        <joint name="car_base_${wheel_name}" type="continuous">
            <origin xyz="${left_right*(car_width)/2.0} 0.0 ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link"/>
            <child link="${wheel_name}"/>
            <axis xyz="1.0 0.0 0.0"/>
        </joint>

        <gazebo reference="${wheel_name}">
            <mu1>0.5</mu1>
            <mu2>0.5</mu2>
        </gazebo>

    </xacro:macro>

    <!-- wheel joint macro -->
    <!-- right:1 left:-1 -->
    <xacro:macro name="wheel_car_joint2" params="wheel_name left_right">
        <link name="${wheel_name}">
            <visual>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </visual>
            <collision>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </collision>
            <xacro:default_inertial mass="1.0"/>
        </link>

        <!-- joints -->
        <joint name="car_base_${wheel_name}" type="continuous">
            <origin xyz="0.0 ${left_right*(car_length)/2.0} ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link"/>
            <child link="${wheel_name}"/>
            <axis xyz="0.0 1.0 0.0"/>
        </joint>

        <gazebo reference="${wheel_name}">
            <mu1>0.5</mu1>
            <mu2>0.5</mu2>
        </gazebo>


    </xacro:macro>

    <xacro:wheel_car_joint wheel_name="front_right_wheel" left_right="1.0"/>
    <xacro:wheel_car_joint wheel_name="front_left_wheel" left_right="-1.0"/>
    <xacro:wheel_car_joint2 wheel_name="end_right_wheel" left_right="1.0"/>
    <xacro:wheel_car_joint2 wheel_name="end_left_wheel" left_right="-1.0"/>

    <!-- base_link -->
    <link name="base_link"/>
    <joint name="base_link_car" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="car_link"/>
    </joint>

    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>50.0</updateRate>
            <leftJoint>car_base_front_left_wheel</leftJoint>
            <rightJoint>car_base_front_right_wheel</rightJoint>
            <wheelSeparation>1.1</wheelSeparation>
            <wheelDiameter>0.4</wheelDiameter>
            <torque>1.0</torque>
            <commandTopic>/front/cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <robotBaseFrame>base_link</robotBaseFrame>
            <publishWheelTF>true</publishWheelTF>
            <publishWheelJointState>true</publishWheelJointState>
            <legecyMode>false</legecyMode>
            <wheelAcceleration>1</wheelAcceleration>
        </plugin>
    </gazebo>

    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>50.0</updateRate>
            <leftJoint>car_base_end_left_wheel</leftJoint>
            <rightJoint>car_base_end_right_wheel</rightJoint>
            <wheelSeparation>1.1</wheelSeparation>
            <wheelDiameter>0.4</wheelDiameter>
            <torque>1.0</torque>
            <commandTopic>/end/cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <robotBaseFrame>base_link</robotBaseFrame>
            <publishWheelTF>true</publishWheelTF>
            <publishWheelJointState>true</publishWheelJointState>
            <legecyMode>false</legecyMode>
            <wheelAcceleration>1</wheelAcceleration>
        </plugin>
    </gazebo>

</robot>

---

<launch>

    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find myrobot)/worlds/myrobot.world"/>
    </include>

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

    <!-- Spawn a robot into Gazebo -->
    <node name="spawn_urdf1" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model car2" />
    <node name="spawn_urdf2" pkg="gazebo_ros" type="spawn_model" args="-file $(find myrobot)/urdf/cloth.urdf -urdf -z 0.705 -model cloth" />

</launch>

<?xml version="1.0"?>
<robot name="rbo" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- variable -->
    <xacro:property name="base_link_height" value="0.1"/>
    <xacro:property name="sqrt_car_number" value="3"/>
    <xacro:property name="car_width" value="1.5"/>
    <xacro:property name="car_length" value="1.5"/>
    <xacro:property name="car_height" value="0.2"/>
    <xacro:property name="wheel_radius" value="0.2"/>
    <xacro:property name="wheel_origin_xyz" value="0.0 0.0 0.0"/>
    <xacro:property name="wheel_origin_rpy" value="0.0 0.0 0.0"/>


    <!-- inertial,geometry,box,sphere macro -->
    <xacro:macro name="default_inertial" params="mass">
        <inertial>
            <mass value="${mass}" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="box_geometry" params="width length height">
        <geometry>
            <box size="${width} ${length} ${height}"/>
        </geometry>
    </xacro:macro>

    <xacro:macro name="sphere_geometry" params="radius">
        <geometry>
            <sphere radius="${radius}"/>
        </geometry>
    </xacro:macro>

    <!-- wheel_link macro-->
    <xacro:macro name="wheel_link" params="wheel_name">
        <link name="${wheel_name}">
            <visual>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </visual>
            <collision>
                <xacro:sphere_geometry radius="${wheel_radius}"/>
                <origin xyz="${wheel_origin_xyz}" rpy="${wheel_origin_rpy}"/>
            </collision>
            <xacro:default_inertial mass="1.0"/>
        </link>

        <gazebo reference="${wheel_name}">
            <mu1>2</mu1>
            <mu2>2</mu2>
        </gazebo>
    </xacro:macro>

    <!-- car_link macro -->
    <xacro:macro name="car_link" params="car_id">
        <link name="car_link_${car_id}">
            <visual>
                <xacro:box_geometry width="${car_width}" length="${car_length}" height="${car_height}"/>
            </visual>
            <collision>
                <xacro:box_geometry width="${car_width}" length="${car_length}" height="${car_height}"/>
            </collision>
            <xacro:default_inertial mass="3.0"/>

        </link>

        <gazebo reference="car_link_${car_id}">
            <mu1>0.5</mu1>
            <mu2>0.5</mu2>
        </gazebo>

        <xacro:wheel_link wheel_name="front_right_wheel_${car_id}"/>
        <xacro:wheel_link wheel_name="front_left_wheel_${car_id}"/>
        <xacro:wheel_link wheel_name="end_right_wheel_${car_id}"/>
        <xacro:wheel_link wheel_name="end_left_wheel_${car_id}"/>

	<!-- joints1 -->
        <joint name="car_base_front_right_wheel_${car_id}" type="continuous">
            <origin xyz="${(car_width)/4.0} 0.0 ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link_${car_id}"/>
            <child link="front_right_wheel_${car_id}"/>
            <axis xyz="0.0 1.0 0.0"/><!--x=1-->
        </joint>

	<!-- joints2 -->
        <joint name="car_base_front_left_wheel_${car_id}" type="continuous">
            <origin xyz="${-1*(car_width)/4.0} 0.0 ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link_${car_id}"/>
            <child link="front_left_wheel_${car_id}"/>
            <axis xyz="0.0 1.0 0.0"/><!--x=1-->
        </joint>

        <!-- joints3 -->
        <joint name="car_base_end_right_wheel_${car_id}" type="continuous">
            <origin xyz="0.0 ${(car_length)/4.0} ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link_${car_id}"/>
            <child link="end_right_wheel_${car_id}"/>
            <axis xyz="0.0 1.0 0.0"/>
        </joint>

        <!-- joints4 -->
        <joint name="car_base_end_left_wheel_${car_id}" type="continuous">
            <origin xyz="0.0 ${-1*(car_length)/4.0} ${(car_height)/2.0+wheel_radius}" rpy="0.0 0.0 0.0"/>
            <parent link="car_link_${car_id}"/>
            <child link="end_left_wheel_${car_id}"/>
            <axis xyz="0.0 1.0 0.0"/>
        </joint>

        <gazebo>
            <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>50.0</updateRate>
                <leftJoint>car_base_front_left_wheel_${car_id}</leftJoint>
                <rightJoint>car_base_front_right_wheel_${car_id}</rightJoint>
                <wheelSeparation>1.1</wheelSeparation>
                <wheelDiameter>0.4</wheelDiameter>
                <torque>1.0</torque>
                <commandTopic>/front/cmd_vel</commandTopic>
                <odometryTopic>odom</odometryTopic>
                <odometryFrame>odom</odometryFrame>
                <robotBaseFrame>car_link_${car_id}</robotBaseFrame>
                <publishWheelTF>true</publishWheelTF>
                <publishWheelJointState>true</publishWheelJointState>
                <legecyMode>false</legecyMode>
                <wheelAcceleration>1</wheelAcceleration>
            </plugin>
        </gazebo>

        <gazebo>
            <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>50.0</updateRate>
                <leftJoint>car_base_end_left_wheel_${car_id}</leftJoint>
                <rightJoint>car_base_end_right_wheel_${car_id}</rightJoint>
                <wheelSeparation>1.1</wheelSeparation>
                <wheelDiameter>0.4</wheelDiameter>
                <torque>1.0</torque>
                <commandTopic>/end/cmd_vel</commandTopic>
                <odometryTopic>odom</odometryTopic>
                <odometryFrame>odom</odometryFrame>
                <robotBaseFrame>car_link_${car_id}</robotBaseFrame>
                <publishWheelTF>true</publishWheelTF>
                <publishWheelJointState>true</publishWheelJointState>
                <legecyMode>false</legecyMode>
                <wheelAcceleration>1</wheelAcceleration>
            </plugin>
        </gazebo>
        
    </xacro:macro>

    <xacro:car_link car_id="1"/>
    <xacro:car_link car_id="2"/>
    <xacro:car_link car_id="3"/>
    <xacro:car_link car_id="4"/>
    <xacro:car_link car_id="5"/>
    <xacro:car_link car_id="6"/>
    <xacro:car_link car_id="7"/>
    <xacro:car_link car_id="8"/>
    <xacro:car_link car_id="9"/>


    <!-- base_link -->
    <link name="base_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <xacro:box_geometry width="${car_width*sqrt_car_number}" length="${car_length*sqrt_car_number}" height="${base_link_height}"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <xacro:box_geometry width="${car_width*sqrt_car_number}" length="${car_length*sqrt_car_number}" height="${base_link_height}"/>
        </collision>
        <xacro:default_inertial mass="3.0"/>
    </link>

    <!-- base_link joint macro -->
    <xacro:macro name="base_car_joint" params="base_car_xyz car_id">
        <joint name="base_link_car_${car_id}" type="fixed">
            <origin xyz="${base_car_xyz}" rpy="0.0 0.0 0.0"/>
            <parent link="base_link"/>
            <child link="car_link_${car_id}"/>
        </joint>
    </xacro:macro>

    <xacro:base_car_joint base_car_xyz="${car_width} ${car_length} ${(base_link_height+car_height)/2}" car_id="1"/>
    <xacro:base_car_joint base_car_xyz="0 ${car_length} ${(base_link_height+car_height)/2}" car_id="2"/>
    <xacro:base_car_joint base_car_xyz="${-1*car_width} ${car_length} ${(base_link_height+car_height)/2}" car_id="3"/>
    <xacro:base_car_joint base_car_xyz="${car_width} 0 ${(base_link_height+car_height)/2}" car_id="4"/>

    <xacro:base_car_joint base_car_xyz="0 0 ${(base_link_height+car_height)/2}" car_id="5"/>
    <xacro:base_car_joint base_car_xyz="${-1*car_width} 0 ${(base_link_height+car_height)/2}" car_id="6"/>
    <xacro:base_car_joint base_car_xyz="${car_width} ${-1*car_length} ${(base_link_height+car_height)/2}" car_id="7"/>
    <xacro:base_car_joint base_car_xyz="0 ${-1*car_length} ${(base_link_height+car_height)/2}" car_id="8"/>
    <xacro:base_car_joint base_car_xyz="${-1*car_width} ${-1*car_length} ${(base_link_height+car_height)/2}" car_id="9"/>

</robot>

<?xml version="1.0"?>
<robot name="cloth" xmlns:xacro="http://ros.org/wiki/xacro">
    <link name="cloth1">
        <visual>
            <geometry>
                <box size="3 3 0.01"/>
            </geometry>
        </visual>
        <collision>
            <geometry>
                <box size="3 3 0.01"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="4" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>
    <gazebo reference="cloth1">
        <material>Gazebo/SkyBlue</material>
        <mu1>2</mu1>
        <mu2>2</mu2>
    </gazebo>
</robot>

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
  </world>
</sdf>