<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from waking_robot.xacro             | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="robot">
  <link name="dummy">
    </link>
  <link name="base_footprint">
    <visual>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <inertial>
      <mass value="0.0001"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <joint name="dummy_joint" type="fixed">
    <parent link="dummy"/>
    <child link="base_footprint"/>
  </joint>
  <!--  	<xacro:include filename="$(find nav_sim)/urdf/robot.gazebo" />  -->
  <gazebo reference="base_footprint">
    <material>Gazebo/Green</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 0"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.2 .3 .1"/>
      </geometry>
      <origin rpy="0 0 1.5707963" xyz="0 0 0.05"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.2 .3 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="8.2"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <link name="wheel_1">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
      <!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
    </collision>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <link name="wheel_2">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
      <!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
    </collision>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <link name="wheel_3">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
      <!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
    </collision>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <link name="wheel_4">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
      <!--	<origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.06"/>
      </geometry>
    </collision>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>
  <joint name="base_to_wheel1" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_1"/>
    <origin rpy="-1.5707963 0 0" xyz="0.1 0.13 0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="base_to_wheel2" type="continuous">
    <axis xyz="0 0 1"/>
    <anchor xyz="0 0 0"/>
    <limit effort="100" velocity="100"/>
    <parent link="base_link"/>
    <child link="wheel_2"/>
    <origin rpy="-1.5707963 0 0" xyz="-0.1 0.13 0"/>
  </joint>
  <joint name="base_to_wheel3" type="continuous">
    <parent link="base_link"/>
    <axis xyz="0 0 1"/>
    <child link="wheel_3"/>
    <origin rpy="-1.5707963 0 0" xyz="0.1 -0.13 0"/>
  </joint>
  <joint name="base_to_wheel4" type="continuous">
    <parent link="base_link"/>
    <axis xyz="0 0 1"/>
    <child link="wheel_4"/>
    <origin rpy="-1.5707963 0 0" xyz="-0.1 -0.13 0"/>
  </joint>
  <!-- IMU joint -->
  <joint name="imu_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0.125 0 0.125"/>
    <parent link="base_link"/>
    <child link="imu_link"/>
  </joint>
  <!-- IMU -->
  <link name="imu_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="1e-2"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <!-- IMU -->
  <!-- Camera joint -->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0.125 0 0.175"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>
  <!-- Camera -->
  <link name="camera_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="1e-2"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <!-- camera -->
  <!-- Hokuyo joint -->
  <joint name="hokuyo_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.125 0.0 0.225"/>
    <parent link="base_link"/>
    <axis xyz="0 1 0"/>
    <child link="laser_frame"/>
  </joint>
  <!-- Hokuyo Laser -->
  <link name="laser_frame">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://plc_bringup/meshes/hokuyo.dae"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1e-2"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <!-- hokuyo -->
  <!-- gazebo插件设置相关 -->
  <gazebo reference="base_link">
    <material>Gazebo/Orange</material>
  </gazebo>
  <gazebo reference="wheel_1">
    <material>Gazebo/Red</material>
  </gazebo>
  <gazebo reference="wheel_2">
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="wheel_3">
    <material>Gazebo/Red</material>
  </gazebo>
  <gazebo reference="wheel_4">
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- ros_control plugin -->
  <!--   <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/robot</robotNamespace>
       <legacyModeNS> true </legacyModeNS>   
    </plugin>
  </gazebo> -->
  <!-- camera_link -->
  <gazebo reference="camera_link">
    <material>Gazebo/Red</material>
  </gazebo>
  <!-- hokuyo -->
  <gazebo reference="laser_frame">
    <sensor name="head_hokuyo_sensor" type="ray">
      <!-- <sensor type="gpu_ray" name="head_hokuyo_sensor">   -->
      <!-- GPU -->
      <always_on>true</always_on>
      <visualize>true</visualize>
      <pose>0 0 0 0 0 0</pose>
      
      <update_rate>15</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926</min_angle>
            <max_angle>3.1415926</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <!-- GPU版本 -->
      <!-- <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">  -->
      <!-- 换一个turtlebot同款 -->
      <!-- <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
        <topicName>/scan</topicName>
        <frameName>laser_frame</frameName>
      </plugin> -->
      <plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
            <ros>
                <!-- <namespace>/tb3</namespace> -->
                <remapping>~/out:=scan</remapping>
            </ros>
            <output_type>sensor_msgs/LaserScan</output_type>
            <frame_name>laser_frame</frame_name>
          </plugin>
    </sensor>
  </gazebo>
  <!-- camera -->
  <gazebo reference="camera_link">
    <sensor name="camera1" type="camera">
      <update_rate>30.0</update_rate>
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1920</width>
          <height>1080</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1]. -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>/</cameraName>
        <imageTopicName>cam</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_link</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!-- Drive controller -->
  <gazebo>
    <plugin filename="libgazebo_ros_skid_steer_drive.so" name="skid_steer_drive_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <robotNamespace>/</robotNamespace>
      <leftFrontJoint>base_to_wheel1</leftFrontJoint>
      <rightFrontJoint>base_to_wheel3</rightFrontJoint>
      <leftRearJoint>base_to_wheel2</leftRearJoint>
      <rightRearJoint>base_to_wheel4</rightRearJoint>
      <wheelSeparation>0.26</wheelSeparation>
      <wheelDiameter>0.12</wheelDiameter>
      <torque>2</torque>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>dummy</robotBaseFrame>
      <broadcastTF>1</broadcastTF>
    </plugin>
  </gazebo>
  <!-- imu控制 -->
  <gazebo reference="imu_link">
    <material>Gazebo/Orange</material>
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <visualize>true</visualize>
      <topic>__default_topic__</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <initial_orientation_as_reference>false</initial_orientation_as_reference>
  
        <topicName>imu</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>100.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>
  <!-- gazebo插件设置相关结束 -->
</robot>
