<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from description/robot.urdf.xacro   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="robot">
  <!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
  <!-- These make use of xacro's mathematical functionality -->
  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>
  <material name="orange">
    <color rgba="1 0.3 0.1 1"/>
  </material>
  <material name="blue">
    <color rgba="0.2 0.2 1 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="red">
    <color rgba="1 0 0 1"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.6 0.6 0.6 1.0"/>
  </material>
  <material name="grey2">
    <color rgba="0.9 0.9 0.9 1.0"/>
  </material>
  <material name="lightGrey">
    <color rgba="0.6 0.6 0.6 1.0"/>
  </material>
  <!-- BASE LINK -->
  <link name="base_link">
    </link>
  <!-- BASE_FOOTPRINT LINK -->
  <joint name="base_footprint_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_footprint"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="base_footprint">
    </link>
  <!-- CHASSIS LINK -->
  <joint name="chassis_joint" type="fixed">
    <parent link="base_link"/>
    <child link="chassis"/>
    <origin xyz="-0.22999999999999998 0 0.01"/>
  </joint>
  <link name="chassis">
    <visual>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0.6 0.33 0.15"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0.6 0.33 0.15"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.5"/>
      <inertia ixx="0.005475000000000001" ixy="0.0" ixz="0.0" iyy="0.0159375" iyz="0.0" izz="0.0195375"/>
    </inertial>
  </link>
  <gazebo reference="chassis">
    <material>Gazebo/Orange</material>
  </gazebo>
  <!-- LEFT WHEEL LINK -->
  <joint name="left_wheel_joint" type="continuous">
    <parent link="chassis"/>
    <child link="left_wheel"/>
    <origin rpy="-1.5707963267948966 0 0" xyz="0.22999999999999998 0.218375 0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder length="0.08" radius="0.135"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.135"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="4.0"/>
      <inertia ixx="0.020358333333333336" ixy="0.0" ixz="0.0" iyy="0.020358333333333336" iyz="0.0" izz="0.03645"/>
    </inertial>
  </link>
  <gazebo reference="left_wheel">
    <material>Gazebo/Blue</material>
  </gazebo>
  <!-- RIGHT WHEEL LINK -->
  <joint name="right_wheel_joint" type="continuous">
    <parent link="chassis"/>
    <child link="right_wheel"/>
    <origin rpy="1.5707963267948966 0 0" xyz="0.22999999999999998 -0.218375 0"/>
    <axis xyz="0 0 -1"/>
  </joint>
  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder length="0.08" radius="0.135"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.135"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="4.0"/>
      <inertia ixx="0.020358333333333336" ixy="0.0" ixz="0.0" iyy="0.020358333333333336" iyz="0.0" izz="0.03645"/>
    </inertial>
  </link>
  <gazebo reference="right_wheel">
    <material>Gazebo/Blue</material>
  </gazebo>
  <!-- CASTER WHEEL LINK -->
  <joint name="caster_wheel_joint" type="fixed">
    <parent link="chassis"/>
    <child link="caster_wheel"/>
    <origin xyz="-0.15 0 -0.115"/>
  </joint>
  <link name="caster_wheel">
    <visual>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="4.0000000000000003e-07" ixy="0.0" ixz="0.0" iyy="4.0000000000000003e-07" iyz="0.0" izz="4.0000000000000003e-07"/>
    </inertial>
  </link>
  <gazebo reference="caster_wheel">
    <material>Gazebo/White</material>
    <mu1 value="0.001"/>
    <mu2 value="0.001"/>
  </gazebo>
  <!-- Lidar LINK -->
  <!-- <joint name="lidar_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="${chassis_length/2 -0.150} 0 ${chassis_height + 0.030}" rpy="0 0 0"/>
    </joint>


    <link name="laser_frame">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link> -->
  <joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_frame"/>
    <origin rpy="0 0 3.141592653589793" xyz="-0.08499999999999999 0 0.18"/>
  </joint>
  <link name="laser_frame">
    <visual>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.01"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="4.0000000000000003e-07" ixy="0.0" ixz="0.0" iyy="4.0000000000000003e-07" iyz="0.0" izz="4.0000000000000003e-07"/>
    </inertial>
  </link>
  <gazebo reference="laser_frame">
    <material>Gazebo/White</material>
  </gazebo>
  <ros2_control name="RealRobot" type="system">
    <hardware>
      <plugin>diffdrive_arduino/DiffDriveArduino</plugin>
      <param name="left_wheel_name">left_wheel_joint</param>
      <param name="right_wheel_name">right_wheel_joint</param>
      <param name="loop_rate">30</param>
      <param name="device">/dev/ttyUSB0</param>
      <param name="baud_rate">57600</param>
      <param name="timeout">1000</param>
      <param name="enc_counts_per_rev">3436</param>
    </hardware>
    <joint name="left_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-10</param>
        <param name="max">10</param>
      </command_interface>
      <state_interface name="velocity"/>
      <state_interface name="position"/>
    </joint>
    <joint name="right_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-10</param>
        <param name="max">10</param>
      </command_interface>
      <state_interface name="velocity"/>
      <state_interface name="position"/>
    </joint>
  </ros2_control>
  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>/home/bjorn/Documents/ros_projects/cps_rmp220_support/install/cps_rmp220_support/share/cps_rmp220_support/config/my_controllers.yaml</parameters>
      <parameters>/home/bjorn/Documents/ros_projects/cps_rmp220_support/install/cps_rmp220_support/share/cps_rmp220_support/config/gaz_ros2_ctl_use_sim.yaml</parameters>
    </plugin>
  </gazebo>
  <joint name="camera_joint" type="fixed">
    <parent link="chassis"/>
    <child link="camera_link"/>
    <origin rpy="0 0 0" xyz="0.276 0 0.181"/>
  </joint>
  <link name="camera_link">
    <visual>
      <geometry>
        <box size="0.010 0.03 0.03"/>
      </geometry>
      <material name="black"/>
    </visual>
    <visual>
      <origin xyz="0 0 -0.05"/>
      <geometry>
        <cylinder length="0.1" radius="0.002"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <joint name="camera_optical_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_link_optical"/>
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
  </joint>
  <link name="camera_link_optical"/>
  <gazebo reference="camera_link">
    <material>Gazebo/Black</material>
    <sensor name="camera" type="camera">
      <pose> 0 0 0 0 0 0 </pose>
      <visualize>true</visualize>
      <update_rate>10</update_rate>
      <camera>
        <horizontal_fov>1.089</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>8.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <frame_name>camera_link_optical</frame_name>
      </plugin>
    </sensor>
  </gazebo>
  <!-- <xacro:include filename="depth_camera.xacro" /> -->
  <joint name="face_joint" type="fixed">
    <parent link="chassis"/>
    <child link="face_link"/>
    <origin rpy="0 0 0" xyz="0.3 0 0.075"/>
  </joint>
  <link name="face_link">
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 0.05 0.01"/>
      <geometry>
        <cylinder length="0.002" radius="0.01"/>
      </geometry>
      <material name="black"/>
    </visual>
    <visual>
      <origin rpy="0 1.5707963267948966 0" xyz="0 -0.05 0.01"/>
      <geometry>
        <cylinder length="0.002" radius="0.01"/>
      </geometry>
      <material name="black"/>
    </visual>
    <visual>
      <origin rpy="0 1.5 0" xyz="-0.011 0 -0.00"/>
      <geometry>
        <cylinder length="0.02" radius="0.05"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <gazebo reference="face_link">
    <material>Gazebo/Black</material>
  </gazebo>
</robot>
