diff --git a/config/twist_mux.yaml b/config/twist_mux.yaml
index 97411fc..5875970 100644
--- a/config/twist_mux.yaml
+++ b/config/twist_mux.yaml
@@ -4,7 +4,7 @@ twist_mux:
       navigation:
         topic   : cmd_vel
         timeout : 0.5
-        priority: 10
+        priority: 100
       tracker:
         topic   : cmd_vel_tracker
         timeout : 0.5
@@ -12,4 +12,4 @@ twist_mux:
       joystick:
         topic   : cmd_vel_joy
         timeout : 0.5
-        priority: 100
\ No newline at end of file
+        priority: 50
\ No newline at end of file
diff --git a/description/robot_core.xacro b/description/robot_core.xacro
index 244abd8..14702fd 100644
--- a/description/robot_core.xacro
+++ b/description/robot_core.xacro
@@ -202,13 +202,37 @@
 
     <!-- Lidar LINK -->
 
-    <joint name="lidar_joint" type="fixed">
+    <!-- <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 xyz="${-0.08} 0 ${chassis_height + 0.030}" rpy="0 0 ${pi}"/>
+    </joint>
+
+
     <link name="laser_frame">
         <visual>
             <geometry>
diff --git a/test.urdf b/test.urdf
new file mode 100644
index 0000000..71e46e0
--- /dev/null
+++ b/test.urdf
@@ -0,0 +1,326 @@
+<?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.08 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>