diff --git a/.gitignore b/.gitignore
index 90e7691..412765c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@ build
 install
 log
 shapes
+src/sherpa/urdf/sherpa.urdf
diff --git a/src/sherpa/urdf/mecanum.sdf b/src/sherpa/urdf/mecanum.sdf
deleted file mode 100644
index 6d39c77..0000000
--- a/src/sherpa/urdf/mecanum.sdf
+++ /dev/null
@@ -1,367 +0,0 @@
-<?xml version="1.0" ?>
-<!--
-  Gazebo Mecanum drive plugin demo
-
-  Try sending commands:
-
-    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 0.5, y: 0.5}"
-
-    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {y: 0.5}, angular: {z: 0.2}"
-
--->
-<sdf version="1.6">
-  <world name="mecanum_drive">
-
-    <physics name="1ms" type="ignored">
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-    </physics>
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-    </plugin>
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>1 1 1 1</diffuse>
-      <specular>0.5 0.5 0.5 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>50</mu>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
-    <model name='vehicle_blue' xmlns:gz="http://gazebosim.org/schema">
-      <pose>0 2 0.325 0 -0 0</pose>
-
-      <link name='chassis'>
-        <pose>-0.151427 -0 0.175 0 -0 0</pose>
-        <inertial>
-          <mass>1.14395</mass>
-          <inertia>
-            <ixx>0.126164</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.416519</iyy>
-            <iyz>0</iyz>
-            <izz>0.481014</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>2.01142 1 0.568726</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>0.5 0.5 1.0 1</ambient>
-            <diffuse>0.5 0.5 1.0 1</diffuse>
-            <specular>0.0 0.0 1.0 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>2.01142 1 0.568726</size>
-            </box>
-          </geometry>
-        </collision>
-      </link>
-
-      <link name='front_left_wheel'>
-        <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='rear_left_wheel'>
-        <pose>-0.957138 0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='front_right_wheel'>
-        <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='rear_right_wheel'>
-        <pose>-0.957138 -0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-
-      <joint name='front_left_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>front_left_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='front_right_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>front_right_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='rear_left_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>rear_left_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='rear_right_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>rear_right_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <plugin
-        filename="gz-sim-mecanum-drive-system"
-        name="gz::sim::systems::MecanumDrive">
-        <front_left_joint>front_left_wheel_joint</front_left_joint>
-        <front_right_joint>front_right_wheel_joint</front_right_joint>
-        <back_left_joint>rear_left_wheel_joint</back_left_joint>
-        <back_right_joint>rear_right_wheel_joint</back_right_joint>
-        <wheel_separation>1.25</wheel_separation>
-        <wheelbase>1.511</wheelbase>
-        <wheel_radius>0.3</wheel_radius>
-        <min_acceleration>-5</min_acceleration>
-        <max_acceleration>5</max_acceleration>
-      </plugin>
-
-    </model>
-
-  </world>
-</sdf>
diff --git a/src/sherpa/urdf/sherpa.urdf b/src/sherpa/urdf/sherpa.urdf
deleted file mode 100644
index 9e5119c..0000000
--- a/src/sherpa/urdf/sherpa.urdf
+++ /dev/null
@@ -1,270 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from /ros2_ws/src/sherpa/urdf/sherpa.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="sherpa" xmlns:gz="http://gazebosim.org">
-  <link name="base_link">
-    <inertial>
-      <mass value="15"/>
-      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
-    </inertial>
-  </link>
-  <material name="mecanum_wheel_material">
-    <ambient>0.2 0.2 0.2 1</ambient>
-    <diffuse>0.2 0.2 0.2 1</diffuse>
-    <specular>0.2 0.2 0.2 1</specular>
-    <emissive>0 0 0 0</emissive>
-    <color rgba="0.2 0.2 0.2 1"/>
-  </material>
-  <link name="lidar_link">
-    <visual name="base">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.0015" radius="0.06"/>
-      </geometry>
-    </visual>
-    <collision name="base_collision">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.01" radius="0.06"/>
-      </geometry>
-    </collision>
-    <visual name="suport_1">
-      <pose>-0.035 0.035 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <visual name="suport_2">
-      <pose>-0.035 -0.035 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <visual name="suport_3">
-      <pose>0.0495 0 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <!-- Bottom cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="bot_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_1.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <!-- Top cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="top_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_2.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <!-- Main cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="main_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_scan.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <inertial>
-      <mass value="0.83"/>
-      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
-    </inertial>
-  </link>
-  <joint name="lidar_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="lidar_link"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-  </joint>
-  <gazebo reference="lidar_link">
-    <sensor name="lidar" type="gpu_lidar">
-      <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
-        <render_engine>ogre2</render_engine>
-      </plugin>
-      <update_rate>10</update_rate>
-      <topic>/velodyne_points</topic>
-      <gz_frame_id>lidar_link</gz_frame_id>
-      <lidar>
-        <scan>
-          <horizontal>
-            <samples>1800.0</samples>
-            <resolution>1</resolution>
-            <min_angle>-3.141592654</min_angle>
-            <max_angle>3.141592654</max_angle>
-          </horizontal>
-          <vertical>
-            <samples>16</samples>
-            <resolution>1</resolution>
-            <min_angle>-0.261799389</min_angle>
-            <max_angle>0.261799389</max_angle>
-          </vertical>
-        </scan>
-        <range>
-          <min>0.01</min>
-          <max>100.0</max>
-          <resolution>0.03</resolution>
-        </range>
-      </lidar>
-    </sensor>
-  </gazebo>
-  <link name="wheel_fr_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base_link">1 1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_fr_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_fr_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz=" 0.11 -0.11 0"/>
-  </joint>
-  <link name="wheel_fl_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base_link">1 -1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_fl_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_fl_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz=" 0.11  0.11 0"/>
-  </joint>
-  <link name="wheel_br_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base_link">1 -1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_br_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_br_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz="-0.11 -0.11 0"/>
-  </joint>
-  <link name="wheel_bl_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base_link">1 1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_bl_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_bl_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz="-0.11  0.11 0"/>
-  </joint>
-  <ros2_control name="SherpaSystem" type="system">
-    <hardware>
-      <plugin>ros2_control/GenericSystemInterface</plugin>
-    </hardware>
-    <joint name="wheel_fl_joint">
-      <command_interface name="velocity"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="wheel_fr_joint">
-      <command_interface name="velocity"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="wheel_bl_joint">
-      <command_interface name="velocity"/>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="wheel_br_joint">
-      <command_interface name="velocity"/>
-      <state_interface name="velocity"/>
-    </joint>
-  </ros2_control>
-</robot>