diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3943d9f44..be9d56c4c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -87,7 +87,7 @@ set(MROVER_CATKIN_PACKAGES
 
 macro(rosify_cpp_target_macro target)
     target_link_libraries(${target} PRIVATE ${catkin_LIBRARIES})
-    target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} "src/util")
+    target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} src/util)
     add_dependencies(${target} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
     target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:CXX>:${MROVER_CPP_COMPILE_OPTIONS}>)
@@ -195,12 +195,6 @@ else ()
             tf2_geometry_msgs
             tf2_ros
             tf2
-            tf2_web_republisher
-            visualization_msgs
-            image_transport
-            cv_bridge
-            rosbridge_server
-            teleop_twist_joy
             gazebo_ros
     )
 
@@ -209,13 +203,14 @@ else ()
             starter_project/autonomy/AutonomyStarterProject.cmake
     )
 
+
     # -=-=-=-=-
     # Macros
     # -=-=-=-=-
 
     # These packages need to be found individually
     macro(add_packages_macro)
-        find_package(OpenCV REQUIRED COMPONENTS core aruco)
+        find_package(OpenCV REQUIRED)
         find_package(gazebo REQUIRED)
         find_package(Eigen3 REQUIRED)
         find_package(ZED 2 QUIET)
@@ -234,9 +229,9 @@ else ()
     # define an add and link macro
     # Put items here to build
     macro(add_and_link_macro)
-        add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/*.cpp" "src/perception/tag_detector")
+        add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/*.cpp src/perception/tag_detector)
         target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp)
-        target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie)
+        target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie)
 
         add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp")
 
@@ -258,8 +253,11 @@ else ()
             target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp)
             target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
             target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie)
-            target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET)
-            target_compile_options(zed_nodelet PRIVATE $<$<COMPILE_LANGUAGE:CUDA>:--expt-extended-lambda>)
+            target_compile_definitions(zed_nodelet PRIVATE
+                    MROVER_IS_NODELET # Generate code relevant to nodelet
+                    ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode
+                    __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore
+            )
         endif ()
     endmacro()
 endif ()
diff --git a/config/localization.yaml b/config/localization.yaml
index 022386712..2d2654e34 100644
--- a/config/localization.yaml
+++ b/config/localization.yaml
@@ -1,7 +1,7 @@
 world_frame: "map"
 odom_frame: "odom"
 rover_frame: "base_link"
-use_odom_frame: true
+use_odom_frame: false
 
 gps_linearization:
   reference_point_latitude: 42.293195
diff --git a/config/rviz/arm.rviz b/config/rviz/arm.rviz
index c1aab6175..f7dac412f 100644
--- a/config/rviz/arm.rviz
+++ b/config/rviz/arm.rviz
@@ -4,12 +4,12 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
+        - /Status1
         - /Grid1
-        - /MotionPlanning1
-        - /MotionPlanning1/Scene Geometry1
-        - /MotionPlanning1/Scene Robot1
+        - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 336
+    Tree Height: 871
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -51,184 +51,60 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 100
-      Reference Frame: world
+      Reference Frame: base_link
       Value: true
-    - Acceleration_Scaling_Factor: 0.1
-      Class: moveit_rviz_plugin/MotionPlanning
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
       Enabled: true
-      Move Group Namespace: ""
-      MoveIt_Allow_Approximate_IK: false
-      MoveIt_Allow_External_Program: false
-      MoveIt_Allow_Replanning: false
-      MoveIt_Allow_Sensor_Positioning: false
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
-      MoveIt_Use_Constraint_Aware_IK: false
-      MoveIt_Workspace:
-        Center:
-          X: 0
-          Y: 0
-          Z: 0
-        Size:
-          X: 2
-          Y: 2
-          Z: 2
-      Name: MotionPlanning
-      Planned Path:
-        Color Enabled: false
-        Interrupt Display: false
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          aTob:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bToc:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          cTod:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          chassisToa:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          dToe:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          eTof:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Loop Animation: false
-        Robot Alpha: 0.5
-        Robot Color: 150; 50; 150
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 3x
-        Trail Step Size: 1
-        Trajectory Topic: move_group/display_planned_path
-        Use Sim Time: false
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-        TextHeight: 0.07999999821186066
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: ""
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
-      Planning Scene Topic: move_group/monitored_planning_scene
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        a_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        b_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        e_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
       Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 0.8999999761581421
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          aTob:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bToc:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          cTod:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          chassisToa:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          dToe:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          eTof:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Robot Alpha: 0.5
-        Show Robot Collision: true
-        Show Robot Visual: false
+      TF Prefix: ""
+      Update Interval: 0
       Value: true
-      Velocity_Scaling_Factor: 0.1
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: chassisToa
-    Frame Rate: 30
+    Fixed Frame: base_link
+    Frame Rate: 60
   Name: root
   Tools:
     - Class: rviz/Interact
@@ -251,7 +127,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 103.44278717041016
+      Distance: 65.38457489013672
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -259,29 +135,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -1.1320419311523438
-        Y: 0.6890945434570312
-        Z: -1.7296075820922852
+        X: 5.594668865203857
+        Y: 0.6798133850097656
+        Z: 3.844665288925171
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.13748864829540253
+      Pitch: 0.42248857021331787
       Target Frame: <Fixed Frame>
-      Yaw: 2.7814881801605225
+      Yaw: 1.0014894008636475
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 948
+  Height: 1085
   Hide Left Dock: false
   Hide Right Dock: true
-  MotionPlanning:
-    collapsed: false
-  MotionPlanning - Trajectory Slider:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000031bfc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000018d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d0000001880000017d00fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001cb00000188000000000000000000000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000031b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -290,6 +162,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 0
-  Y: 27
+  Width: 1574
+  X: 177
+  Y: 89
diff --git a/config/rviz/auton_sim.rviz b/config/rviz/auton_sim.rviz
index f4c4076fb..8ed1993dc 100644
--- a/config/rviz/auton_sim.rviz
+++ b/config/rviz/auton_sim.rviz
@@ -3,9 +3,11 @@ Panels:
     Help Height: 85
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Perception1
+        - /Perception1/ZED Camera1
       Splitter Ratio: 0.5
-    Tree Height: 689
+    Tree Height: 549
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -51,6 +53,8 @@ Visualization Manager:
       Value: true
     - Class: rviz/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
@@ -66,7 +70,7 @@ Visualization Manager:
           Value: false
         center_right_wheel_link:
           Value: false
-        course1:
+        chassis_link:
           Value: true
         front_left_wheel_link:
           Value: false
@@ -76,6 +80,10 @@ Visualization Manager:
           Value: false
         map:
           Value: true
+        right_camera_link:
+          Value: true
+        zed2i_left_camera_frame:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -85,24 +93,27 @@ Visualization Manager:
       Tree:
         map:
           base_link:
-            back_left_wheel_link:
-              {}
-            back_right_wheel_link:
-              {}
-            camera_link:
-              {}
-            center_left_wheel_link:
-              {}
-            center_right_wheel_link:
-              {}
-            front_left_wheel_link:
-              {}
-            front_right_wheel_link:
-              {}
-            imu_link:
+            chassis_link:
+              back_left_wheel_link:
+                {}
+              back_right_wheel_link:
+                {}
+              camera_link:
+                {}
+              center_left_wheel_link:
+                {}
+              center_right_wheel_link:
+                {}
+              front_left_wheel_link:
+                {}
+              front_right_wheel_link:
+                {}
+              imu_link:
+                {}
+              right_camera_link:
+                {}
+            zed2i_left_camera_frame:
               {}
-          course1:
-            {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -129,7 +140,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         camera_link:
           Alpha: 1
           Show Axes: false
@@ -145,6 +155,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         front_left_wheel_link:
           Alpha: 1
           Show Axes: false
@@ -159,6 +174,11 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        right_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: Rover Model
       Robot Description: robot_description
       TF Prefix: ""
@@ -214,7 +234,7 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.009999999776482582
           Style: Flat Squares
-          Topic: /camera/depth/points
+          Topic: /camera/left/points
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
@@ -227,8 +247,8 @@ Visualization Manager:
           Min Value: 0
           Name: ZED Camera
           Normalize Range: true
-          Queue Size: 2
-          Transport Hint: compressed
+          Queue Size: 1
+          Transport Hint: raw
           Unreliable: false
           Value: true
       Enabled: true
@@ -285,7 +305,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 16.141542434692383
+      Distance: 9.403231620788574
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -301,17 +321,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6602029204368591
+      Pitch: 0.6652029156684875
       Target Frame: <Fixed Frame>
-      Yaw: 1.8885608911514282
+      Yaw: 1.958560824394226
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1271
+  Height: 1074
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000015a00000450fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000347000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000014005a00450044002000430061006d0065007200610100000392000001020000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000d520000003efc0100000002fb0000000800540069006d0065010000000000000d52000003a200fffffffb0000000800540069006d0065010000000000000450000000000000000000000bf10000045000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015a00000394fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000014005a00450044002000430061006d00650072006101000002fa000000d70000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000066c0000003efc0100000002fb0000000800540069006d006501000000000000066c000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000050c0000039400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -320,8 +340,8 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 3410
-  X: 12
-  Y: 154
+  Width: 1644
+  X: 1057
+  Y: 85
   ZED Camera:
     collapsed: false
diff --git a/launch/arm_control.launch b/launch/arm_control.launch
index 519bfc9e9..ff33ff1b2 100644
--- a/launch/arm_control.launch
+++ b/launch/arm_control.launch
@@ -3,9 +3,9 @@
 	Positions will be published to /ra_cmd 
 -->
 <launch>
-	<include file="$(find mrover_arm)/launch/demo.launch">
-		<arg name="moveit_controller_manager" value="simple" />
-    </include>
-	
-	<node name="mrover_arm_follow_joint_trajectory" pkg="mrover" type="arm_trajectory"></node>
-</launch>
\ No newline at end of file
+  <include file="$(find mrover_arm)/launch/demo.launch">
+    <arg name="moveit_controller_manager" value="simple"/>
+  </include>
+
+  <node name="mrover_arm_follow_joint_trajectory" pkg="mrover" type="arm_trajectory"/>
+</launch>
diff --git a/launch/arm_gazebo_sim.launch b/launch/arm_gazebo_sim.launch
index 10f43ffbe..0c405c87e 100644
--- a/launch/arm_gazebo_sim.launch
+++ b/launch/arm_gazebo_sim.launch
@@ -3,9 +3,9 @@ Launch File to open Gazebo sim for arm,
 positions published to /ra_cmd and /gazebo_arm_controller/command
 -->
 <launch>
-	<rosparam command="load" file="$(find mrover)/config/teleop.yaml" />
-	<include file="$(find mrover_arm)/launch/demo_gazebo.launch">
-    </include>
-	
-	<node name="mrover_arm_follow_joint_trajectory" pkg="mrover" type="arm_trajectory_server.py"/>
+  <rosparam command="load" file="$(find mrover)/config/teleop.yaml"/>
+  <include file="$(find mrover_arm)/launch/demo_gazebo.launch">
+  </include>
+
+  <node name="mrover_arm_follow_joint_trajectory" pkg="mrover" type="arm_trajectory_server.py"/>
 </launch>
\ No newline at end of file
diff --git a/launch/arm_rviz_display.launch b/launch/arm_rviz_display.launch
index 1e54558d3..7bf41022d 100644
--- a/launch/arm_rviz_display.launch
+++ b/launch/arm_rviz_display.launch
@@ -1,16 +1,16 @@
 <!-- 
-  This launch file is for viewing the arm URDF and moving it around 
-with the MoveIt motion planning panel in rviz
+  This launch file is for viewing the arm URDF in RViz.
 -->
 <launch>
-  <arg name="model" default="$(find mrover)/rover_description/arm/arm.urdf" />
-  <arg name="gui" default="false" />
-  <param name="robot_description" command="xacro $(find mrover)/rover_description/arm/arm.xacro" />
-  <arg name="rvizconfig" default="$(find mrover)/config/rviz/arm.rviz" />
+  <arg name="gui" default="false"/>
+  <arg name="rvizconfig" default="$(find mrover)/config/rviz/arm.rviz"/>
 
-  <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
-  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+  <param name="robot_description" command="xacro $(find mrover)/urdf/arm/arm.urdf.xacro"/>
 
-</launch>
\ No newline at end of file
+  <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
+  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
+</launch>
diff --git a/launch/auton.launch b/launch/auton.launch
index 41c256bc8..763657241 100644
--- a/launch/auton.launch
+++ b/launch/auton.launch
@@ -1,58 +1,50 @@
 <!-- This launch file launches all nodes necessary for autonomous navigation. -->
 <launch>
-    <arg name="run_tag_detector" default="true"/>
-    <arg name="sim" default="false"/>
-    <arg name="use_ekf" default="true"/>
-    <arg name="ekf_start_delay" default="0"/>
+  <arg name="run_tag_detector" default="true"/>
+  <arg name="sim" default="false"/>
+  <arg name="use_ekf" default="true"/>
+  <arg name="ekf_start_delay" default="0"/>
 
-    <!--
-      ==========
-      Perception
-      ==========
-    -->
-    <node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" respawn="true"
-          args="manager" output="screen"/>
-    <!-- nodelet to detect AR tags and publish them to the TF tree -->
-    <node if="$(arg run_tag_detector)"
-          pkg="nodelet" type="nodelet" name="tag_detector" respawn="true"
-          args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen"/>
+  <!--
+    ==========
+    Perception
+    ==========
+  -->
+  <node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" respawn="true"
+        args="manager" output="screen"/>
+  <!-- nodelet to detect AR tags and publish them to the TF tree -->
+  <node if="$(arg run_tag_detector)"
+        pkg="nodelet" type="nodelet" name="tag_detector" respawn="true"
+        args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen"/>
 
-    <!--
-      ===========
-      Navigation
-      ===========
-    -->
-    <!-- node to navigate to a series of waypoints, AR tags, and gates -->
-    <rosparam command="load" file="$(find mrover)/config/navigation.yaml"/>
-    <node name="nav" pkg="mrover" type="navigation.py"/>
-    <node name="failure_id" pkg="mrover" type="failure_identification.py"/>
+  <!--
+    ===========
+    Navigation
+    ===========
+  -->
+  <!-- node to navigate to a series of waypoints, AR tags, and gates -->
+  <rosparam command="load" file="$(find mrover)/config/navigation.yaml"/>
+  <node name="nav" pkg="mrover" type="navigation.py"/>
+  <node name="failure_id" pkg="mrover" type="failure_identification.py"/>
 
-    <!--
-      ============
-      Localization
-      ============
-    -->
-    <rosparam command="load" file="$(find mrover)/config/localization.yaml"/>
-    <rosparam if="$(arg sim)" command="load" file="$(find mrover)/config/sim_ekf.yaml"/>
-    <rosparam unless="$(arg sim)" command="load" file="$(find mrover)/config/ekf.yaml"/>
+  <!--
+    ============
+    Localization
+    ============
+  -->
+  <rosparam command="load" file="$(find mrover)/config/localization.yaml"/>
+  <rosparam if="$(arg sim)" command="load" file="$(find mrover)/config/sim_ekf.yaml"/>
+  <rosparam unless="$(arg sim)" command="load" file="$(find mrover)/config/ekf.yaml"/>
 
-    <!-- global EKF -->
-    <node if="$(arg use_ekf)" pkg="robot_localization" type="ekf_localization_node" name="global_ekf"
-          clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'">
-        <remap from="odometry/filtered" to="global_ekf/odometry"/>
-    </node>
+  <!-- global EKF -->
+  <node if="$(arg use_ekf)" pkg="robot_localization" type="ekf_localization_node" name="global_ekf"
+        clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'">
+    <remap from="odometry/filtered" to="global_ekf/odometry"/>
+  </node>
 
-    <!-- passthrough filter to replace EKF in datapath -->
-    <node unless="$(arg use_ekf)" pkg="mrover" type="passthrough_filter.py" name="passthrough_filter"/>
+  <!-- passthrough filter to replace EKF in datapath -->
+  <node unless="$(arg use_ekf)" pkg="mrover" type="passthrough_filter.py" name="passthrough_filter"/>
 
-    <!-- node to linearize GPS from geodetic to ENU cartesian coords -->
-    <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen"/>
-
-    <!-- plot the rover trajectory in rviz as it drives -->
-    <!-- <node name="hector_trajectory_server" output="screen" pkg="hector_trajectory_server" type="hector_trajectory_server">
-        <param name="target_frame_name" type="string" value="map"/>
-        <param name="source_frame_name" type="string" value="base_link"/>
-        <param name="trajectory_update_rate" type="double" value="4"/>
-        <param name="trajectory_publish_rate" type="double" value="4"/>
-    </node>-->
+  <!-- node to linearize GPS from geodetic to ENU cartesian coords -->
+  <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen"/>
 </launch>
diff --git a/launch/auton_sim.launch b/launch/auton_sim.launch
index 24cd8017d..37cfaa6ea 100644
--- a/launch/auton_sim.launch
+++ b/launch/auton_sim.launch
@@ -3,8 +3,7 @@
  -->
 <launch>
     <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
-    <arg name="world_name" default="world"/>
-    <arg name="use_odom" default="true"/>
+    <arg name="world_name" default="world_flat"/>
 
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
 
@@ -16,37 +15,13 @@
         <arg name="world_name" value="$(arg world_name)"/>
     </include>
 
-    <!-- visual odometry node to provide localization in odom frame -->
-    <node if="$(arg use_odom)"
-          pkg="nodelet" type="nodelet" name="rtabmap_stereo_odometry" respawn="true"
-          args="load rtabmap_odom/stereo_odometry perception_nodelet_manager" output="screen">
-        <param name="frame_id" type="string" value="base_link"/>
-        <param name="odom_frame_id" type="string" value="odom"/>
-
-        <remap from="/odom" to="/odometry"/>
-        <remap from="/left/image_rect" to="/camera/left/image"/>
-        <remap from="/left/camera_info" to="/camera/left/camera_info"/>
-        <remap from="/right/image_rect" to="/camera/right/image"/>
-        <remap from="/right/camera_info" to="/camera/right/camera_info"/>
-
-        <param name="GFTT/MinDistance" type="string" value="10"/>
-        <param name="OdomF2M/MaxSize" type="string" value="500"/>
-        <param name="Vis/MaxFeatures" type="string" value="400"/>
-        <!-- Correspondences: 0=Features Matching, 1=Optical Flow -->
-        <param name="Vis/CorType" value="0"/>
-        <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) -->
-        <param name="Odom/Strategy" value="0"/>
-
-        <param name="publish_tf" type="bool" value="true"/>
-    </node>
-
-    <node pkg="tf" type="static_transform_publisher" name="map_odom_publisher"
-          args="1 0 0 0 0 0 base_link zed2i_left_camera_frame 100" />
+    <node pkg="tf" type="static_transform_publisher" name="zed2i_left_camera_frame_publisher"
+          args="0 0 0 0 0 0 1 left_camera_link zed2i_left_camera_frame 100"/>
 
     <!-- launch auton -->
     <include file="$(find mrover)/launch/auton.launch">
         <arg name="sim" value="true"/>
         <arg name="use_ekf" value="true"/>
-        <arg name="ekf_start_delay" value="10.0"/>
+        <arg name="ekf_start_delay" value="5"/>
     </include>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/launch/rover_core.launch b/launch/rover_core.launch
index d3551cc41..234de4e60 100644
--- a/launch/rover_core.launch
+++ b/launch/rover_core.launch
@@ -3,7 +3,7 @@
  -->
 <launch>
     <!-- load our rover's model -->
-    <param name="robot_description" command="xacro $(find mrover)/rover_description/rover.xacro" />
+    <param name="robot_description" command="xacro $(find mrover)/urdf/rover/rover.urdf.xacro" />
 
     <!-- publish the state of each joint as a transform in the TF tree -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
diff --git a/launch/simulator.launch b/launch/simulator.launch
index ac63fadb0..bb1929a46 100644
--- a/launch/simulator.launch
+++ b/launch/simulator.launch
@@ -5,33 +5,33 @@
     :arg gazebo_config_path: path to the Gazebo config containing Gazebo environment and world description
  -->
 <launch>
-    <arg name="gui" default="true" />
-    <arg name="gazebo_config_path" default="$(find mrover)/config/gazebo" />
-    <arg name="world_name" default="world" />
+  <arg name="gui" default="true"/>
+  <arg name="gazebo_config_path" default="$(find mrover)/config/gazebo"/>
+  <arg name="world_name" default="world"/>
 
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <env name="GAZEBO_MODEL_PATH" value="$(arg gazebo_config_path)/env_description" />
-        <arg name="world_name" value="$(arg gazebo_config_path)/env_description/$(arg world_name).world" />
-        <arg name="paused" value="false" />
-        <arg name="use_sim_time" value="true" />
-        <arg name="gui" value="$(arg gui)" />
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <env name="GAZEBO_MODEL_PATH" value="$(arg gazebo_config_path)/env_description"/>
+    <arg name="world_name" value="$(arg gazebo_config_path)/env_description/$(arg world_name).world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="$(arg gui)"/>
 
-        <!-- run gazebo server in debug mode -->
-        <arg name="debug" value="false" />
-        <arg name="verbose" default="false" />
+    <!-- run gazebo server in debug mode -->
+    <arg name="debug" value="false"/>
+    <arg name="verbose" default="false"/>
 
-        <!-- if the gazebo GUI or server closes or dies, the whole launch file will die -->
-        <arg name="server_required" value="true" />
-        <arg name="gui_required" value="true" />
-    </include>
+    <!-- if the gazebo GUI or server closes or dies, the whole launch file will die -->
+    <arg name="server_required" value="true"/>
+    <arg name="gui_required" value="true"/>
+  </include>
 
-    <!-- merge imu & mag msgs from gazebo into one -->
-    <node name="imu_packager" pkg="mrover" type="sim_imu_driver.py" />
+  <!-- merge imu & mag msgs from gazebo into one -->
+  <node name="imu_packager" pkg="mrover" type="sim_imu_driver.py"/>
 
-    <!-- spawn the rover model in gazebo, requires robot_description rosparam to be set -->
-    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen" 
-          args="-urdf -param robot_description -model rover" />
+  <!-- spawn the rover model in gazebo, requires robot_description rosparam to be set -->
+  <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen"
+        args="-urdf -param robot_description -model rover"/>
 
-    <!-- publish the state of each joint as a JointState message -->
-    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-</launch>
\ No newline at end of file
+  <!-- publish the state of each joint as a JointState message -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+</launch>
diff --git a/launch/zed.launch b/launch/zed.launch
index 302833fbc..fb10687f3 100644
--- a/launch/zed.launch
+++ b/launch/zed.launch
@@ -19,7 +19,7 @@
           args="0 0 0 0 0 0 base_link zed_mount_link 100"/>
 
     <!-- Load ZED URDF -->
-    <param name="zed_description" command="$(find xacro)/xacro '$(find mrover)/rover_description/zed/zed_descr.urdf.xacro'
+    <param name="zed_description" command="$(find xacro)/xacro '$(find mrover)/urdf/zed/zed_descr.urdf.xacro'
         camera_name:=$(arg zed_model)
         camera_model:=$(arg zed_model)"/>
 
diff --git a/package.xml b/package.xml
index e87ee3c2c..87ca4865b 100644
--- a/package.xml
+++ b/package.xml
@@ -28,13 +28,13 @@
   <!--   <build_depend>roscpp</build_depend> -->
   <!--   <exec_depend>roscpp</exec_depend> -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <build_depend>message_generation</build_depend>
+  <!--  <build_depend>message_generation</build_depend>-->
   <!-- Use build_export_depend for packages you need in order to build against this package: -->
   <!--   <build_export_depend>message_generation</build_export_depend> -->
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use exec_depend for packages you need at runtime: -->
-  <exec_depend>message_runtime</exec_depend>
+  <!--  <exec_depend>message_runtime</exec_depend>-->
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
@@ -42,35 +42,25 @@
 
   <!-- Common -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>rospy</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>nodelet</exec_depend>
+  <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nodelet</depend>
+  <build_depend>message_generation</build_depend>
   <exec_depend>xacro</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>rosbridge_server</exec_depend>
-  <exec_depend>tf2_web_republisher</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
   <!-- Visualization -->
-  <depend>rviz</depend>
-  <depend>hector_trajectory_server</depend>
+  <exec_depend>rviz</exec_depend>
 
   <!-- Perception -->
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>image_transport</depend>
-  <depend>theora_image_transport</depend>
-  <depend>compressed_image_transport</depend>
-  <depend>compressed_depth_image_transport</depend>
   <depend>sensor_msgs</depend>
   <depend>dynamic_reconfigure</depend>
 
@@ -84,12 +74,12 @@
   <depend>control_msgs</depend>
 
   <!-- Teleop -->
-  <depend>teleop_twist_joy</depend>
+  <exec_depend>teleop_twist_joy</exec_depend>
 
   <!-- Simulation -->
   <depend>gazebo_ros</depend>
-  <depend>hector_gazebo</depend>
-  <depend>teleop_twist_keyboard</depend>
+  <exec_depend>hector_gazebo</exec_depend>
+  <exec_depend>teleop_twist_keyboard</exec_depend>
 
   <!-- Test -->
   <test_depend>rosunit</test_depend>
diff --git a/rover_description/arm/a-b.stl b/rover_description/arm/a-b.stl
deleted file mode 100644
index 57ac63efb..000000000
--- a/rover_description/arm/a-b.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:b3c30a8bfc29eda1759324b64fdce651113d37c987d5bbe3ac65b68fb83271e7
-size 2805084
diff --git a/rover_description/arm/arm.xacro b/rover_description/arm/arm.xacro
deleted file mode 100644
index a366f1bb8..000000000
--- a/rover_description/arm/arm.xacro
+++ /dev/null
@@ -1,199 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mrover_arm">
-    <virtual_joint name="" type="fixed" parent_frame="world" child_link="chassis-a" />
-
-    <xacro:property name="PI" value="3.141592" />
-    <xacro:property name="base_width" value="7" />
-    <xacro:property name="base_len" value="7" />
-    <xacro:property name="base_height" value="19" />
-
-    <xacro:macro name="link-visual" params="fileName x y z col r g b roll:=0 pitch:=0 yaw:=0">
-        <visual>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}" />
-            <geometry>
-                <mesh filename="package://mrover/rover_description/arm/${fileName}" />
-            </geometry>
-            <material name="${col}">
-                <color rgba="${r} ${g} ${b} 1" />
-            </material>
-        </visual>
-    </xacro:macro>
-
-    <xacro:macro name="link-inertial" params="x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 mass ixx iyy izz ixy:=0 iyz:=0 ixz:=0">
-        <inertial>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}" />
-            <!-- mass in kg -->
-            <mass value="${mass}" />
-            <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="${ixy}" iyz="${iyz}" ixz="${ixz}" />
-        </inertial>
-    </xacro:macro>
-
-    <xacro:macro name="box_inertia" params="m w h d">
-    <inertial>
-      <mass value="${m}" />
-      <inertia ixx="${m / 12.0 * (d*d + h*h)}" ixy="0.0" ixz="0.0" iyy="${m / 12.0 * (w*w + h*h)}" iyz="0.0" izz="${m / 12.0 * (w*w + d*d)}" />
-    </inertial>
-  </xacro:macro>
-
-    <!-- defines a cylinder collision object with two endpoints -->
-    <xacro:macro name="link-collision-cylinder" params="radius x1 y1 z1 x2 y2 z2 roll:=0 pitch:=0 yaw:=0">
-        <xacro:property name="dx" value="${x2-x1}" />
-        <xacro:property name="dy" value="${y2-y1}" />
-        <xacro:property name="dz" value="${z2-z1}" />
-
-        <xacro:if value="${dx != 0}">
-            <xacro:property name="length" value="${fabs(dx)}" />
-            <xacro:property name="rpy" value="${roll} ${pitch+PI/2} ${yaw}" />
-        </xacro:if> 
-        <xacro:if value="${dy != 0}">
-            <xacro:property name="length" value="${fabs(dy)}" />
-            <xacro:property name="rpy" value="${roll+PI/2} ${pitch} ${yaw}" />
-        </xacro:if>
-        <xacro:if value="${dz != 0}">
-            <xacro:property name="length" value="${fabs(dz)}" />
-            <xacro:property name="rpy" value="${roll} ${pitch} ${yaw}" />
-        </xacro:if>
-
-        <collision>
-            <origin xyz="${x1+dx/2} ${y1+dy/2} ${z1+dz/2}" rpy="${rpy}" />
-            <geometry>
-                <cylinder radius="${radius}" length="${length}" />
-            </geometry>
-        </collision>
-    </xacro:macro>
-
-    <!-- defines a box collision object with the center and dimensions -->
-    <xacro:macro name="link-collision-box" params="x:=0 y:=0 z:=0 lx ly lz roll:=0 pitch:=0 yaw:=0">
-        <collision>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}" />
-            <geometry>
-                <box size="${lx} ${ly} ${lz}" />
-            </geometry>
-        </collision>
-    </xacro:macro>
-
-    <xacro:macro name="joints" params="name type:=revolute parent child roll:=0 pitch:=0 yaw:=0 origin_x origin_y origin_z axis_x:=0 axis_y:=0 axis_z:=0 lower upper damp:=1.0" >
-        <joint name="${name}" type="${type}">
-            <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="${roll} ${pitch} ${yaw}" />
-            <parent link="${parent}" />
-            <child link="${child}" />
-            <axis xyz="${axis_x} ${axis_y} ${axis_z}" />
-            <!-- effort in N-m, velocity in rad/s -->
-            <limit lower="${lower}" upper="${upper}" effort="30" velocity="1" />
-            <!-- damping in N-m-s/rad -->
-            <dynamics damping="${damp}" />
-        </joint>
-    </xacro:macro>
-
-    <xacro:macro name="gazebo_ref" params="link">
-        <gazebo reference="${link}">
-            <material>Gazebo/White</material>
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <selfCollide>false</selfCollide>
-        </gazebo>
-   </xacro:macro>
-
-    <xacro:macro name="transmissions" params="name">
-        <transmission name="trans_${name}">
-            <type>transmission_interface/SimpleTransmission</type>
-            <joint name="${name}">
-                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-            </joint>
-            <actuator name="${name}_motor">
-                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-                <mechanicalReduction>1</mechanicalReduction>
-            </actuator>
-        </transmission>
-    </xacro:macro>
-
-
-    <link name="base_link">
-        <xacro:box_inertia m="3.0" w="${base_len}" h="${base_width}" d="${base_height}" />
-        <xacro:link-visual fileName="rover.stl" x="0" y="0" z="-9.5" col="red" r="1" g="1" b="1"/>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 -9.5" />
-            <geometry>
-                <mesh filename="package://mrover/rover_description/arm/rover.stl" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chassisToa">
-        <xacro:link-visual fileName="chassis-a.stl" x="0" y="0" z="0" col="red" r="1" g="0" b="0"/>
-        <xacro:link-collision-cylinder radius="3.75" x1="0" y1="0" z1="-1.25" x2="0" y2="0" z2="-3.75" />
-        <xacro:link-collision-cylinder radius="1.75" x1="3.6467207" y1="3.6590503" z1="-0.75" x2="3.6467207" y2="3.6590503" z2="-2.6913988" />
-        <!-- calculated based on cylinder1 -->
-        <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.3" iyy="2.3" izz="3.5" />
-    </link>
-    <link name="aTob">
-        <xacro:link-visual fileName="a-b.stl" x="0" y="0" z="0" col="orange" r="1" g="0.43" b="0" roll="0"/>
-        <xacro:link-collision-box y="-1.75" z="3" lx="2" ly="3" lz="6" />
-        <xacro:link-collision-cylinder radius="2" x1="0" y1="0" z1="-0.25" x2="0" y2="0" z2="0.75" />
-        <xacro:link-inertial y="-1.75" z="3" mass="0.5" ixx="1.9" iyy="0.7" izz="0.5" />
-    </link>
-    <link name="bToc">
-        <xacro:link-visual fileName="b-c.stl" x="0" y="0" z="0" col="yellow" r="1" g="1" b="0" roll="0"/>
-        <xacro:link-collision-box y="9" z="5" lx="2" ly="20" lz="2" roll="0.48" />
-        <xacro:link-collision-cylinder radius="2" x1="-2" y1="20.25" z1="10.75" x2="4.5" y2="20.25" z2="10.75" />
-        <xacro:link-collision-cylinder radius="1" x1="0" y1="-2" z1="8.5" x2="0" y2="17" z2="8.5" roll="0.3" />
-        <xacro:link-inertial y="12" mass="0.5" ixx="24" iyy="0.75" izz="24" />
-    </link>
-    <link name="cTod">
-        <xacro:link-visual fileName="c-d.stl" x="0" y="0" z="0" col="green" r="0.43" g="1" b="0"/>
-        <xacro:link-collision-cylinder radius="3.75" x1="-4.5" y1="0" z1="-2.5" x2="-4.5" y2="0" z2="3" roll="-0.17"/>
-        <xacro:link-inertial x="-1" mass="0.5" ixx="3.2" iyy="3.2" izz="16" />
-    </link>
-    <link name="dToe">
-        <xacro:link-visual fileName="d-e.stl" x="0" y="0" z="0" col="blue" r="0" g="0" b="1" pitch="0" roll="0"/>
-        <xacro:link-collision-box y="0" z="-7" lx="4" ly="20" lz="4" roll="${(-PI/2.0)+0.05}"/>
-        <xacro:link-collision-cylinder radius="2.5" x1="-3" y1="0" z1="-17" x2="3" y2="0" z2="-17" />
-        <xacro:link-inertial y="-7.6324" z="4.7693" roll="-0.558505" mass="0.5" ixx="17" iyy="0.5" izz="17" />
-    </link>
-    <link name="eTof">
-        <xacro:link-visual fileName="e-f.stl" x="0" y="0" z="0" col="violet" r="0.5" g="0" b="1" roll="0" />
-        <xacro:link-collision-cylinder radius="3" x1="3" y1="1.8" z1="0" x2="-6.5" y2="1.8" z2="0" />
-        <xacro:link-inertial z="-3" mass="0.5" ixx="1.6" iyy="1.6" izz="0.25" />
-    </link>
-    <link name="hand">
-        <xacro:link-visual fileName="hand.stl" x="0" y="0" z="0" col="pink" r="1" g="0" b="1"/>
-        <xacro:link-collision-box x="-2.25" y="1" z="0.5" lx="8" ly="3.5" lz="8.5" roll="${PI/2.0}"/>
-        <xacro:link-collision-box z="8.75" y="3.5" x="-1" lx="3.5" ly="3.5" lz="5" roll="${PI/2.0}"/>
-        <xacro:link-collision-cylinder radius="0.75" x1="0" y1="2.3" z1="0" x2="0" y2="2.3" z2="9"/>
-        <xacro:link-collision-box x="-1.6" y="-1.5" z="-2" lx="1" ly="2.5" lz="2.5"/>
-        <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.2" iyy="4" izz="2.2" />
-    </link>
-
-    <link name="world" />
-    <joint name="virtual_joint" type="fixed">
-        <parent link="world" />
-        <child link="base_link" />
-        <origin xyz="0 0 ${base_height/2.0}" rpy="0 0 0" />
-    </joint>
-
-    <xacro:joints name="base_link_connection" type="fixed" parent="base_link" child="chassisToa" origin_x="0" origin_y="15.55" origin_z="11" axis_z="0" lower="0" upper="0" />
-    <xacro:joints name="joint_a" type="continuous" parent="chassisToa" child="aTob" origin_x="0" origin_y="0" origin_z="0" axis_z="1" lower="-3.14" upper="3.14" />
-    <xacro:joints name="joint_b" parent="aTob" child="bToc" origin_x="0" origin_y="1.33" origin_z="0.99" axis_x="1" lower="-1.064651" upper="0" roll="0.593412"/>
-    <xacro:joints name="joint_c" parent="bToc" child="cTod" origin_x="0" origin_y="20.11" origin_z="10.75" axis_x="1" lower="-0.087267" upper="4.886922" roll="-0.436332"/>
-    <xacro:joints name="joint_d" parent="cTod" child="dToe" origin_x="-4.4" origin_y="-0.42" origin_z="-2.535" axis_z="1" axis_y="0" lower="-3.14" upper="3.14" roll="-0.15708" pitch="-0.0174533" yaw="0" />
-    <xacro:joints name="joint_e" parent="dToe" child="eTof" origin_x="2.55" origin_y="0.2" origin_z="-17" axis_y="1" lower="-3.59538" upper="0.610865" roll="0" pitch="0" yaw="-1.649336"/>
-    <xacro:joints name="joint_f" parent="eTof" child="hand" origin_x="-6.3" origin_y="1.75" origin_z="0" axis_x="1" lower="-0.523599" upper="6.28" />
-    <xacro:transmissions name="joint_a" />
-    <xacro:transmissions name="joint_b" />
-    <xacro:transmissions name="joint_c" />
-    <xacro:transmissions name="joint_d" />
-    <xacro:transmissions name="joint_e" />
-    <xacro:transmissions name="joint_f" />
-
-    <xacro:gazebo_ref link="chassisToa" />
-    <xacro:gazebo_ref link="aTob" />
-    <xacro:gazebo_ref link="bToc" />
-    <xacro:gazebo_ref link="cTod" />
-    <xacro:gazebo_ref link="dToe" />
-    <xacro:gazebo_ref link="eTof" />
-    <xacro:gazebo_ref link="hand" />
-
-    <gazebo>
-        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
-    </gazebo>
-    
-</robot>
\ No newline at end of file
diff --git a/rover_description/arm/b-c.stl b/rover_description/arm/b-c.stl
deleted file mode 100644
index e31f631fd..000000000
--- a/rover_description/arm/b-c.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:a7abf243bf1b82354387cffbb0ecf4af7bea7ec701c117b754ed78e6b2e96b6f
-size 4979884
diff --git a/rover_description/arm/c-d.stl b/rover_description/arm/c-d.stl
deleted file mode 100644
index 8bcc84e3f..000000000
--- a/rover_description/arm/c-d.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:e637d12085beeb75d07d1f794b610ac4a1c6d4b2808a3ca0fa2125178d3b156a
-size 3619284
diff --git a/rover_description/arm/chassis-a.stl b/rover_description/arm/chassis-a.stl
deleted file mode 100644
index 1536d73de..000000000
--- a/rover_description/arm/chassis-a.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:f01776d1cdaa6c80b6d458488ed138d95f712a1bd88df90f8bfe64e9a65b1d66
-size 5415784
diff --git a/rover_description/arm/d-e.stl b/rover_description/arm/d-e.stl
deleted file mode 100644
index 446f347fe..000000000
--- a/rover_description/arm/d-e.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:b2ee87af5aa0143486ddb848838396ab86c91ed899c6b06f13c0c44732f36b39
-size 5820184
diff --git a/rover_description/arm/e-f.stl b/rover_description/arm/e-f.stl
deleted file mode 100644
index e2204d9a7..000000000
--- a/rover_description/arm/e-f.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:ce14608a1e6b0a8df6c8f196b6bb21fb760c6e23d062cfc7df10235cb125083d
-size 2572384
diff --git a/rover_description/arm/hand.stl b/rover_description/arm/hand.stl
deleted file mode 100644
index 8f9f68cd8..000000000
--- a/rover_description/arm/hand.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:13180570fcc75330948c53d8b78ab10dc87dddbab0da728c04dc1ffe3393c465
-size 4091484
diff --git a/rover_description/arm/rover.stl b/rover_description/arm/rover.stl
deleted file mode 100644
index 6219c2ebe..000000000
--- a/rover_description/arm/rover.stl
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:8550292cabe3ed779084e4ee25f887f07fc1a264c657396cdb190b19f278fb84
-size 20406384
diff --git a/rover_description/meshes/rover.blend b/rover_description/meshes/rover.blend
deleted file mode 100644
index 66eaf4b4e..000000000
--- a/rover_description/meshes/rover.blend
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:9d6cd6335f71dd2e5402de41b3cdbcee08b0765a412e2a0b3c8d5f51bb638cef
-size 52422704
diff --git a/rover_description/meshes/rover.dae b/rover_description/meshes/rover.dae
deleted file mode 100644
index 01d5f3025..000000000
--- a/rover_description/meshes/rover.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:9e561cc4b0a3aa7855fa1fa212aa77e56fb7c2189dc92b870394d1794e6e125e
-size 79737736
diff --git a/rover_description/meshes/wheel.blend b/rover_description/meshes/wheel.blend
deleted file mode 100644
index fa31e3f42..000000000
--- a/rover_description/meshes/wheel.blend
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:cee2d7008dcd1126bcbb13fd2e98e6de3c15d4971f9db45279909467ce43cb61
-size 4253540
diff --git a/rover_description/meshes/wheel.dae b/rover_description/meshes/wheel.dae
deleted file mode 100644
index e2f60a837..000000000
--- a/rover_description/meshes/wheel.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:4e4f01271497c32c3cea2783076bae87f0d9bd89a65c4ab2473f2e09c164f778
-size 5372316
diff --git a/src/gazebo/differential_drive_6w.cpp b/src/gazebo/differential_drive_6w.cpp
index 9e1ae3ce4..c93eb5dee 100644
--- a/src/gazebo/differential_drive_6w.cpp
+++ b/src/gazebo/differential_drive_6w.cpp
@@ -32,7 +32,6 @@
  */
 
 #include "differential_drive_6w.hpp"
-#include "mrover/MotorsStatus.h"
 
 #include <algorithm>
 
@@ -45,6 +44,8 @@
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_listener.h>
 
+#include <mrover/MotorsStatus.h>
+
 namespace gazebo {
 
     constexpr auto VELOCITY_COMMAND_TOPIC = "cmd_vel";
diff --git a/src/gazebo/gazebo_ros_openni_kinect.h b/src/gazebo/gazebo_ros_openni_kinect.h
deleted file mode 100644
index b2a0cc701..000000000
--- a/src/gazebo/gazebo_ros_openni_kinect.h
+++ /dev/null
@@ -1,193 +0,0 @@
-/*
- * Copyright (C) 2012-2014 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-/*
- * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor.
- * Author: John Hsu
- * Date: 24 Sept 2008
- */
-
-#ifndef GAZEBO_ROS_OPENNI_KINECT_HH
-#define GAZEBO_ROS_OPENNI_KINECT_HH
-
-// ros stuff
-#include <ros/advertise_options.h>
-#include <ros/callback_queue.h>
-#include <ros/ros.h>
-
-// ros messages stuff
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/fill_image.h>
-#include <std_msgs/Float64.h>
-
-// gazebo stuff
-#include <gazebo/common/Time.hh>
-#include <gazebo/msgs/MessageTypes.hh>
-#include <gazebo/physics/physics.hh>
-#include <gazebo/plugins/DepthCameraPlugin.hh>
-#include <gazebo/sensors/SensorTypes.hh>
-#include <gazebo/transport/TransportTypes.hh>
-#include <sdf/Param.hh>
-
-// dynamic reconfigure stuff
-#include <dynamic_reconfigure/server.h>
-#include <gazebo_plugins/GazeboRosOpenniKinectConfig.h>
-
-// boost stuff
-#include <boost/thread/mutex.hpp>
-
-// camera stuff
-#include <gazebo_plugins/gazebo_ros_camera_utils.h>
-
-namespace gazebo {
-    class GazeboRosOpenniKinect : public DepthCameraPlugin, GazeboRosCameraUtils {
-        /// \brief Constructor
-        /// \param parent The parent entity, must be a Model or a Sensor
-    public:
-        GazeboRosOpenniKinect();
-
-        /// \brief Destructor
-    public:
-        ~GazeboRosOpenniKinect();
-
-        /// \brief Load the plugin
-        /// \param take in SDF root element
-    public:
-        virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
-
-        /// \brief Advertise point cloud and depth image
-    public:
-        virtual void Advertise();
-
-        /// \brief Update the controller
-    protected:
-        virtual void OnNewDepthFrame(const float* _image,
-                                     unsigned int _width, unsigned int _height,
-                                     unsigned int _depth, const std::string& _format);
-
-        /// \brief Update the controller
-    protected:
-        virtual void OnNewImageFrame(const unsigned char* _image,
-                                     unsigned int _width, unsigned int _height,
-                                     unsigned int _depth, const std::string& _format);
-
-        /// \brief push point cloud data into ros topic
-    private:
-        void FillPointdCloud(const float* _src);
-
-        /// \brief push depth image data into ros topic
-    private:
-        void FillDepthImage(const float* _src);
-
-        /// \brief Keep track of number of connctions for point clouds
-    private:
-        int point_cloud_connect_count_;
-
-    private:
-        void PointCloudConnect();
-
-    private:
-        void PointCloudDisconnect();
-
-        /// \brief Keep track of number of connctions for point clouds
-    private:
-        int depth_image_connect_count_;
-
-    private:
-        void DepthImageConnect();
-
-    private:
-        void DepthImageDisconnect();
-
-    private:
-        bool FillPointCloudHelper(sensor_msgs::PointCloud2& point_cloud_msg,
-                                  uint32_t rows_arg, uint32_t cols_arg,
-                                  uint32_t step_arg, void* data_arg);
-
-    private:
-        bool FillDepthImageHelper(sensor_msgs::Image& image_msg,
-                                  uint32_t height, uint32_t width,
-                                  uint32_t step, void* data_arg);
-
-        /// \brief A pointer to the ROS node.  A node will be instantiated if it does not exist.
-    private:
-        ros::Publisher point_cloud_pub_;
-
-    private:
-        ros::Publisher depth_image_pub_;
-
-        /// \brief PointCloud2 point cloud message
-    private:
-        sensor_msgs::PointCloud2 point_cloud_msg_;
-
-    private:
-        sensor_msgs::Image depth_image_msg_;
-
-        /// \brief Minimum range of the point cloud
-    private:
-        double point_cloud_cutoff_;
-        /// \brief Maximum range of the point cloud
-    private:
-        double point_cloud_cutoff_max_;
-
-        /// \brief ROS image topic name
-    private:
-        std::string point_cloud_topic_name_;
-
-
-        /// \brief image where each pixel contains the depth data
-    private:
-        std::string depth_image_topic_name_;
-
-    private:
-        common::Time depth_sensor_update_time_;
-
-        // overload with our own
-    private:
-        std::string depth_image_camera_info_topic_name_;
-
-    private:
-        int depth_info_connect_count_;
-
-    private:
-        void DepthInfoConnect();
-
-    private:
-        void DepthInfoDisconnect();
-
-    private:
-        common::Time last_depth_image_camera_info_update_time_;
-
-    private:
-        bool use_depth_image_16UC1_format_;
-
-    protected:
-        ros::Publisher depth_image_camera_info_pub_;
-
-        using GazeboRosCameraUtils::PublishCameraInfo;
-
-    protected:
-        virtual void PublishCameraInfo();
-
-    private:
-        event::ConnectionPtr load_connection_;
-    };
-
-} // namespace gazebo
-#endif
diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp
index 538fcd197..894c75349 100644
--- a/src/perception/tag_detector/pch.hpp
+++ b/src/perception/tag_detector/pch.hpp
@@ -19,9 +19,9 @@
 #include <opencv2/imgproc.hpp>
 
 #include <dynamic_reconfigure/server.h>
-#include <image_transport/image_transport.h>
 #include <nodelet/loader.h>
 #include <nodelet/nodelet.h>
+#include <sensor_msgs/Image.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp
index 7daea870b..73698714d 100644
--- a/src/perception/tag_detector/tag_detector.cpp
+++ b/src/perception/tag_detector/tag_detector.cpp
@@ -5,8 +5,8 @@ namespace mrover {
     void TagDetectorNodelet::onInit() {
         mNh = getMTNodeHandle();
         mPnh = getMTPrivateNodeHandle();
-        mDetectorParams = new cv::aruco::DetectorParameters();
-        auto defaultDetectorParams = cv::aruco::DetectorParameters::create();
+        mDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
+        auto defaultDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
         int dictionaryNumber;
 
         mNh.param<bool>("use_odom_frame", mUseOdom, false);
@@ -15,16 +15,14 @@ namespace mrover {
         mNh.param<std::string>("camera_frame", mCameraFrameId, "zed2i_left_camera_frame");
 
         mPnh.param<bool>("publish_images", mPublishImages, true);
-        using DictEnumType = std::underlying_type_t<cv::aruco::PREDEFINED_DICTIONARY_NAME>;
-        mPnh.param<int>("dictionary", dictionaryNumber, static_cast<DictEnumType>(cv::aruco::DICT_4X4_50));
+        mPnh.param<int>("dictionary", dictionaryNumber, static_cast<int>(cv::aruco::DICT_4X4_50));
         mPnh.param<int>("min_hit_count_before_publish", mMinHitCountBeforePublish, 5);
         mPnh.param<int>("max_hit_count", mMaxHitCount, 5);
         mPnh.param<int>("tag_increment_weight", mTagIncrementWeight, 2);
         mPnh.param<int>("tag_decrement_weight", mTagDecrementWeight, 1);
 
-        mIt.emplace(mNh);
-        mImgPub = mIt->advertise("tag_detection", 1);
-        mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber);
+        mImgPub = mNh.advertise<sensor_msgs::Image>("tag_detection", 1);
+        mDictionary = cv::makePtr<cv::aruco::Dictionary>(cv::aruco::getPredefinedDictionary(dictionaryNumber));
 
         mPcSub = mNh.subscribe("camera/left/points", 1, &TagDetectorNodelet::pointCloudCallback, this);
         mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this);
diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp
index 616405033..b14541478 100644
--- a/src/perception/tag_detector/tag_detector.hpp
+++ b/src/perception/tag_detector/tag_detector.hpp
@@ -13,13 +13,12 @@ namespace mrover {
     private:
         ros::NodeHandle mNh, mPnh;
 
-        std::optional<image_transport::ImageTransport> mIt;
-        image_transport::Publisher mImgPub;
-        std::unordered_map<int, image_transport::Publisher> mThreshPubs; // Map from threshold scale to publisher
+        ros::Publisher mImgPub;
+        std::unordered_map<int, ros::Publisher> mThreshPubs; // Map from threshold scale to publisher
         ros::ServiceServer mServiceEnableDetections;
 
         ros::Subscriber mPcSub;
-        image_transport::Subscriber mImgSub;
+        ros::Subscriber mImgSub;
         tf2_ros::Buffer mTfBuffer;
         tf2_ros::TransformListener mTfListener{mTfBuffer};
         tf2_ros::TransformBroadcaster mTfBroadcaster;
diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp
index b1033d61d..82866f8ea 100644
--- a/src/perception/tag_detector/tag_detector.processing.cpp
+++ b/src/perception/tag_detector/tag_detector.processing.cpp
@@ -133,7 +133,7 @@ namespace mrover {
                 for (auto& [id, tag]: mTags) {
                     cv::Scalar color{255, 0, 0};
                     cv::Point pt{tagBoxWidth * tagCount, mImg.rows / 10};
-                    std::string text = "id" + std::to_string(id) + ":" + std::to_string((tag.hitCount));
+                    std::string text = "id" + std::to_string(id) + ":" + std::to_string(tag.hitCount);
                     cv::putText(mImg, text, pt, cv::FONT_HERSHEY_COMPLEX, mImg.cols / 800.0, color, mImg.cols / 300);
 
                     ++tagCount;
diff --git a/src/perception/tag_detector/tag_detector.threshold.cpp b/src/perception/tag_detector/tag_detector.threshold.cpp
index b368df16c..263376190 100644
--- a/src/perception/tag_detector/tag_detector.threshold.cpp
+++ b/src/perception/tag_detector/tag_detector.threshold.cpp
@@ -26,7 +26,7 @@ namespace mrover {
             auto it = mThreshPubs.find(scale);
             if (it == mThreshPubs.end()) {
                 ROS_INFO("Creating new publisher for thresholded scale %d", scale);
-                std::tie(it, std::ignore) = mThreshPubs.emplace(scale, mIt->advertise("tag_detection_threshold_" + std::to_string(scale), 1));
+                std::tie(it, std::ignore) = mThreshPubs.emplace(scale, mNh.advertise<sensor_msgs::Image>("tag_detection_threshold_" + std::to_string(scale), 1));
             }
             auto& [_, publisher] = *it;
 
diff --git a/src/perception/zed_wrapper/pch.hpp b/src/perception/zed_wrapper/pch.hpp
index dd801f422..6ff40073f 100644
--- a/src/perception/zed_wrapper/pch.hpp
+++ b/src/perception/zed_wrapper/pch.hpp
@@ -12,13 +12,12 @@
 #include <sl/Camera.hpp>
 #include <thrust/device_vector.h>
 
-#include <image_transport/image_transport.h>
-#include <image_transport/publisher.h>
 #include <nodelet/loader.h>
 #include <nodelet/nodelet.h>
 #include <ros/init.h>
 #include <ros/node_handle.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/PointCloud2.h>
diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp
index 05809ceec..c19353c8f 100644
--- a/src/perception/zed_wrapper/zed_wrapper.cpp
+++ b/src/perception/zed_wrapper/zed_wrapper.cpp
@@ -37,9 +37,8 @@ namespace mrover {
             mMagPub = mNh.advertise<sensor_msgs::MagneticField>("mag", 1);
             mLeftCamInfoPub = mNh.advertise<sensor_msgs::CameraInfo>("camera/left/camera_info", 1);
             mRightCamInfoPub = mNh.advertise<sensor_msgs::CameraInfo>("camera/right/camera_info", 1);
-            image_transport::ImageTransport it{mNh};
-            mLeftImgPub = it.advertise("camera/left/image", 1);
-            mRightImgPub = it.advertise("camera/right/image", 1);
+            mLeftImgPub = mNh.advertise<sensor_msgs::Image>("camera/left/image", 1);
+            mRightImgPub = mNh.advertise<sensor_msgs::Image>("camera/right/image", 1);
 
             std::string grabResolutionString;
             mPnh.param("grab_resolution", grabResolutionString, std::string{sl::toString(sl::RESOLUTION::HD720)});
diff --git a/src/perception/zed_wrapper/zed_wrapper.hpp b/src/perception/zed_wrapper/zed_wrapper.hpp
index db3f71cbb..d252851bb 100644
--- a/src/perception/zed_wrapper/zed_wrapper.hpp
+++ b/src/perception/zed_wrapper/zed_wrapper.hpp
@@ -30,8 +30,7 @@ namespace mrover {
         tf2_ros::Buffer mTfBuffer;
         tf2_ros::TransformListener mTfListener{mTfBuffer};
         tf2_ros::TransformBroadcaster mTfBroadcaster;
-        ros::Publisher mPcPub, mImuPub, mMagPub, mLeftCamInfoPub, mRightCamInfoPub;
-        image_transport::Publisher mLeftImgPub, mRightImgPub;
+        ros::Publisher mPcPub, mImuPub, mMagPub, mLeftCamInfoPub, mRightCamInfoPub, mLeftImgPub, mRightImgPub;
 
         PointCloudGpu mPointCloudGpu;
 
diff --git a/src/teleop/mrover_arm/launch/planning_context.launch b/src/teleop/mrover_arm/launch/planning_context.launch
index 9c4831e59..ac7f4504e 100644
--- a/src/teleop/mrover_arm/launch/planning_context.launch
+++ b/src/teleop/mrover_arm/launch/planning_context.launch
@@ -6,7 +6,7 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find mrover)/rover_description/arm/arm.xacro'"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find mrover)/urdf/arm/arm.xacro'"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic" textfile="$(find mrover_arm)/config/mrover_arm.srdf" />
diff --git a/src/teleoperation/frontend/.gitignore b/src/teleoperation/frontend/.gitignore
index 38adffa64..692d47f6d 100644
--- a/src/teleoperation/frontend/.gitignore
+++ b/src/teleoperation/frontend/.gitignore
@@ -13,6 +13,7 @@ dist
 dist-ssr
 coverage
 *.local
+bun.lockb
 
 /cypress/videos/
 /cypress/screenshots/
diff --git a/starter_project/autonomy/config/starter_project.rviz b/starter_project/autonomy/config/starter_project.rviz
index 31d1b36ae..ca7a99f10 100644
--- a/starter_project/autonomy/config/starter_project.rviz
+++ b/starter_project/autonomy/config/starter_project.rviz
@@ -9,7 +9,7 @@ Panels:
         - /RobotModel1
         - /Image1
       Splitter Ratio: 0.5
-    Tree Height: 796
+    Tree Height: 537
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -77,7 +77,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         camera_link:
           Alpha: 1
           Show Axes: false
@@ -93,6 +92,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        chassis_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         front_left_wheel_link:
           Alpha: 1
           Show Axes: false
@@ -107,6 +111,11 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        right_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -115,6 +124,8 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
@@ -130,12 +141,16 @@ Visualization Manager:
           Value: true
         center_right_wheel_link:
           Value: true
+        chassis_link:
+          Value: true
         front_left_wheel_link:
           Value: true
         front_right_wheel_link:
           Value: true
         imu_link:
           Value: true
+        right_camera_link:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -144,22 +159,25 @@ Visualization Manager:
       Show Names: true
       Tree:
         base_link:
-          back_left_wheel_link:
-            {}
-          back_right_wheel_link:
-            {}
-          camera_link:
-            {}
-          center_left_wheel_link:
-            {}
-          center_right_wheel_link:
-            {}
-          front_left_wheel_link:
-            {}
-          front_right_wheel_link:
-            {}
-          imu_link:
-            {}
+          chassis_link:
+            back_left_wheel_link:
+              {}
+            back_right_wheel_link:
+              {}
+            camera_link:
+              {}
+            center_left_wheel_link:
+              {}
+            center_right_wheel_link:
+              {}
+            front_left_wheel_link:
+              {}
+            front_right_wheel_link:
+              {}
+            imu_link:
+              {}
+            right_camera_link:
+              {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -173,14 +191,14 @@ Visualization Manager:
       Value: false
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /camera/right/image
+      Image Topic: /camera/left/image
       Max Value: 1
       Median window: 5
       Min Value: 0
       Name: Image
       Normalize Range: true
-      Queue Size: 2
-      Transport Hint: compressed
+      Queue Size: 1
+      Transport Hint: raw
       Unreliable: false
       Value: true
     - Alpha: 1
@@ -274,7 +292,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.978219032287598
+      Distance: 3.6909778118133545
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -282,27 +300,27 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.5774051547050476
-        Y: 0.20844532549381256
-        Z: 0.04878166690468788
+        X: 0.1978837251663208
+        Y: 0.135016530752182
+        Z: 0.33381450176239014
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.40020281076431274
+      Pitch: 0.5552026629447937
       Target Frame: <Fixed Frame>
-      Yaw: 5.461760997772217
+      Yaw: 5.666762828826904
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1380
+  Height: 1047
   Hide Left Dock: false
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000004cafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003e60000011f0000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000d2b0000003efc0100000002fb0000000800540069006d0065010000000000000d2b0000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000bcf000004ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e7000000cf0000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006f80000003efc0100000002fb0000000800540069006d00650100000000000006f8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000059c0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -311,6 +329,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 3371
-  X: 69
-  Y: 28
+  Width: 1784
+  X: 159
+  Y: 125
diff --git a/starter_project/autonomy/launch/starter_project.launch b/starter_project/autonomy/launch/starter_project.launch
index a0b8ce28c..d169e1c61 100644
--- a/starter_project/autonomy/launch/starter_project.launch
+++ b/starter_project/autonomy/launch/starter_project.launch
@@ -1,29 +1,28 @@
 <launch>
-  <arg name="model" default="$(find mrover)/rover_description/rover.urdf" />
-  <arg name="gui" default="false" />
-  <param name="robot_description" command="xacro $(find mrover)/rover_description/rover.xacro" />
-  <arg name="rvizconfig" default="$(find mrover)/starter_project/autonomy/config/starter_project.rviz" />
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+  <arg name="gui" default="false"/>
+  <param name="robot_description" command="xacro $(find mrover)/urdf/rover/rover.urdf.xacro"/>
+  <arg name="rvizconfig" default="$(find mrover)/starter_project/autonomy/config/starter_project.rviz"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
 
-  <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
-  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+  <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
+  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <env name="GAZEBO_MODEL_PATH" value="$(find mrover)/config/gazebo/env_description:$(find mrover)/starter_project/autonomy/config" />
-    <arg name="world_name" value="$(find mrover)/starter_project/autonomy/config/starter_project.world" />
-    <arg name="paused" value="false" />
-    <arg name="use_sim_time" value="true" />
-    <arg name="gui" value="true" />
-    <arg name="headless" value="false" />
-    <arg name="debug" value="false" />
-    <arg name="verbose" default="false" />
-    <arg name="gui_required" value="true" />
+    <env name="GAZEBO_MODEL_PATH" value="$(find mrover)/config/gazebo/env_description:$(find mrover)/starter_project/autonomy/config"/>
+    <arg name="world_name" value="$(find mrover)/starter_project/autonomy/config/starter_project.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+    <arg name="verbose" default="false"/>
+    <arg name="gui_required" value="true"/>
   </include>
-starter_project/
+
   <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen" args="
     -urdf -param robot_description
-    -model rover" />
+    -model rover"/>
 
   <!--
     ==========
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index 2185a5fe2..c8f78b7c1 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -17,19 +17,19 @@ int main(int argc, char** argv) {
 
 namespace mrover {
 
-    Perception::Perception() : mNodeHandle{}, mImageTransport{mNodeHandle} {
+    Perception::Perception() : mNodeHandle{} {
         // Subscribe to camera image messages
         // Every time another node publishes to this topic we will be notified
         // Specifically the callback we passed will be invoked
-        mImageSubscriber = mImageTransport.subscribe("camera/right/image", 1, &Perception::imageCallback, this);
+        mImageSubscriber = mNodeHandle.subscribe("camera/right/image", 1, &Perception::imageCallback, this);
 
         // Create a publisher for our tag topic
         // See: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
         // TODO: uncomment me!
         // mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
 
-        mTagDetectorParams = cv::aruco::DetectorParameters::create();
-        mTagDictionary = cv::aruco::getPredefinedDictionary(0);
+        mTagDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
+        mTagDictionary = cv::makePtr<cv::aruco::Dictionary>(cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50));
     }
 
     void Perception::imageCallback(sensor_msgs::ImageConstPtr const& image) {
diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp
index d690e2486..82f2d8b25 100644
--- a/starter_project/autonomy/src/perception.hpp
+++ b/starter_project/autonomy/src/perception.hpp
@@ -12,7 +12,6 @@
 #include <opencv2/core/mat.hpp>
 
 // ROS Headers, ros namespace
-#include <image_transport/image_transport.h>
 #include <ros/node_handle.h>
 #include <ros/publisher.h>
 #include <sensor_msgs/Image.h>
@@ -36,8 +35,7 @@ namespace mrover {
     class Perception {
     private:
         ros::NodeHandle mNodeHandle;
-        image_transport::ImageTransport mImageTransport;
-        image_transport::Subscriber mImageSubscriber;
+        ros::Subscriber mImageSubscriber;
         cv::Ptr<cv::aruco::DetectorParameters> mTagDetectorParams;
         cv::Ptr<cv::aruco::Dictionary> mTagDictionary;
         std::vector<std::vector<cv::Point2f>> mTagCorners;
diff --git a/urdf/arm/arm.urdf.xacro b/urdf/arm/arm.urdf.xacro
new file mode 100644
index 000000000..62106e436
--- /dev/null
+++ b/urdf/arm/arm.urdf.xacro
@@ -0,0 +1,150 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arm">
+  <xacro:property name="PI" value="3.141592"/>
+  <xacro:property name="base_width" value="7"/>
+  <xacro:property name="base_len" value="7"/>
+  <xacro:property name="base_height" value="19"/>
+
+  <!-- Functions -->
+
+  <xacro:macro name="joints"
+               params="name type:=revolute parent child x y z rx:=0 ry:=0 rz:=0 axis_x:=0 axis_y:=0 axis_z:=0 lower upper damp:=1.0">
+    <joint name="${name}" type="${type}">
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${rz}"/>
+      <parent link="${parent}"/>
+      <child link="${child}"/>
+      <axis xyz="${axis_x} ${axis_y} ${axis_z}"/>
+      <!-- effort in N-m, velocity in rad/s -->
+      <limit lower="${lower}" upper="${upper}" effort="30" velocity="1"/>
+      <!-- damping in N-m-s/rad -->
+      <dynamics damping="${damp}"/>
+    </joint>
+  </xacro:macro>
+
+  <xacro:macro name="link-visual" params="fileName col r g b x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
+    <visual>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <geometry>
+        <mesh filename="package://mrover/urdf/meshes/${fileName}"/>
+      </geometry>
+      <material name="${col}">
+        <color rgba="${r} ${g} ${b} 1"/>
+      </material>
+    </visual>
+  </xacro:macro>
+
+  <xacro:macro name="link-inertial"
+               params="x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 mass ixx iyy izz ixy:=0 iyz:=0 ixz:=0">
+    <inertial>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <!-- mass in kg -->
+      <mass value="${mass}"/>
+      <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="${ixy}" iyz="${iyz}" ixz="${ixz}"/>
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="box_inertia" params="m w h d">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia ixx="${m / 12.0 * (d*d + h*h)}" ixy="0.0" ixz="0.0" iyy="${m / 12.0 * (w*w + h*h)}" iyz="0.0"
+               izz="${m / 12.0 * (w*w + d*d)}"/>
+    </inertial>
+  </xacro:macro>
+
+  <!-- defines a cylinder collision object with two endpoints -->
+  <xacro:macro name="link-collision-cylinder" params="radius x1 y1 z1 x2 y2 z2 roll:=0 pitch:=0 yaw:=0">
+    <xacro:property name="dx" value="${x2-x1}"/>
+    <xacro:property name="dy" value="${y2-y1}"/>
+    <xacro:property name="dz" value="${z2-z1}"/>
+
+    <xacro:if value="${dx != 0}">
+      <xacro:property name="length" value="${fabs(dx)}"/>
+      <xacro:property name="rpy" value="${roll} ${pitch+PI/2} ${yaw}"/>
+    </xacro:if>
+    <xacro:if value="${dy != 0}">
+      <xacro:property name="length" value="${fabs(dy)}"/>
+      <xacro:property name="rpy" value="${roll+PI/2} ${pitch} ${yaw}"/>
+    </xacro:if>
+    <xacro:if value="${dz != 0}">
+      <xacro:property name="length" value="${fabs(dz)}"/>
+      <xacro:property name="rpy" value="${roll} ${pitch} ${yaw}"/>
+    </xacro:if>
+
+    <collision>
+      <origin xyz="${x1+dx/2} ${y1+dy/2} ${z1+dz/2}" rpy="${rpy}"/>
+      <geometry>
+        <cylinder radius="${radius}" length="${length}"/>
+      </geometry>
+    </collision>
+  </xacro:macro>
+
+  <!-- defines a box collision object with the center and dimensions -->
+  <xacro:macro name="link-collision-box" params="x:=0 y:=0 z:=0 lx ly lz roll:=0 pitch:=0 yaw:=0">
+    <collision>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <geometry>
+        <box size="${lx} ${ly} ${lz}"/>
+      </geometry>
+    </collision>
+  </xacro:macro>
+
+  <!-- Joints -->
+
+  <xacro:joints name="base_link_joint" type="prismatic" parent="base_link" child="a_link"
+                axis_y="1"
+                x="0" y="-10" z="0"
+                lower="0" upper="20"/>
+  <xacro:joints name="a_joint" parent="a_link" child="b_link"
+                axis_y="1"
+                x="1.32728" y="0.02613" z="1.99693"
+                lower="${-PI / 2}" upper="0"/>
+  <xacro:joints name="b_joint" parent="b_link" child="c_link"
+                axis_y="1"
+                x="22.8544" y="1.81734" z="0.378175"
+                lower="0" upper="${PI}"/>
+  <xacro:joints name="c_joint" parent="c_link" child="d_link"
+                axis_y="1"
+                x="19.5129" y="0.264652" z="0.03498"
+                lower="${-PI}" upper="${PI}"/>
+  <xacro:joints name="d_joint" parent="d_link" child="e_link"
+                axis_x="1"
+                x="5.59352" y="-2.05037" z="0.040519"
+                lower="${-PI}" upper="${PI}"/>
+
+  <!-- Links -->
+
+  <link name="base_link">
+    <xacro:box_inertia m="3.0" w="${base_len}" h="${base_width}" d="${base_height}"/>
+    <xacro:link-visual fileName="chasis.dae" col="red" r="1" g="1" b="1"/>
+    <!--    <collision>-->
+    <!--        <origin rpy="0 0 0" xyz="0 0 -9.5"/>-->
+    <!--        <geometry>-->
+    <!--            <mesh filename="package://robo_impls/urdf/rover.dae"/>-->
+    <!--        </geometry>-->
+    <!--    </collision> -->
+  </link>
+  <link name="a_link">
+    <xacro:link-visual fileName="link.a.dae"
+                       col="white" r="1" g="1" b="1"/>
+    <!--    <xacro:link-collision-cylinder radius="3.75" x1="0" y1="0" z1="-1.25" x2="0" y2="0" z2="-3.75"/>-->
+    <!--    <xacro:link-collision-cylinder radius="1.75" x1="3.6467207" y1="3.6590503" z1="-0.75" x2="3.6467207"-->
+    <!--                                   y2="3.6590503" z2="-2.6913988"/>-->
+    <!--    <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.3" iyy="2.3" izz="3.5"/>-->
+  </link>
+  <link name="b_link">
+    <xacro:link-visual fileName="link.b.dae"
+                       col="orange" r="1" g="0.43" b="0"/>
+  </link>
+  <link name="c_link">
+    <xacro:link-visual fileName="link.c.dae"
+                       col="yellow" r="1" g="1" b="0"/>
+  </link>
+  <link name="d_link">
+    <xacro:link-visual fileName="link.d.dae"
+                       col="green" r="0.43" g="1" b="0"/>
+  </link>
+  <link name="e_link">
+    <xacro:link-visual fileName="link.e.dae"
+                       col="blue" r="0" g="0" b="1"/>
+  </link>
+</robot>
diff --git a/rover_description/meshes/aluminum_base.png b/urdf/meshes/aluminum_base.png
similarity index 100%
rename from rover_description/meshes/aluminum_base.png
rename to urdf/meshes/aluminum_base.png
diff --git a/rover_description/meshes/aluminum_normal.png b/urdf/meshes/aluminum_normal.png
similarity index 100%
rename from rover_description/meshes/aluminum_normal.png
rename to urdf/meshes/aluminum_normal.png
diff --git a/urdf/meshes/arm.dae b/urdf/meshes/arm.dae
new file mode 100644
index 000000000..1c7af67d9
--- /dev/null
+++ b/urdf/meshes/arm.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:48ebba2feeaf3e4acd22c6b46deb6e0cf3237c185b48f142ffc26913e00cdf06
+size 6492
diff --git a/urdf/meshes/chasis.dae b/urdf/meshes/chasis.dae
new file mode 100644
index 000000000..a2dd3f711
--- /dev/null
+++ b/urdf/meshes/chasis.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ea6d616980906bf035d90e563685f2aa3c218be2f47b1c8924884922114e81aa
+size 6491
diff --git a/urdf/meshes/link.a.dae b/urdf/meshes/link.a.dae
new file mode 100644
index 000000000..192bf4e09
--- /dev/null
+++ b/urdf/meshes/link.a.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5be28f75de929d3ee54a86985c9d38fc34f8ae3f92ab87b3fe7ae7aef0bee797
+size 10815714
diff --git a/urdf/meshes/link.b.dae b/urdf/meshes/link.b.dae
new file mode 100644
index 000000000..4d93afdbf
--- /dev/null
+++ b/urdf/meshes/link.b.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bfaa73edb0cb66f9298f9b68323386d6a203a49b78e8a2b12f5d349754b74126
+size 4232741
diff --git a/urdf/meshes/link.c.dae b/urdf/meshes/link.c.dae
new file mode 100644
index 000000000..db082c9e6
--- /dev/null
+++ b/urdf/meshes/link.c.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:480da3567990aa3302b406789b5cce05c113e4da0353d79180fb46afb612bc23
+size 9013569
diff --git a/urdf/meshes/link.d.dae b/urdf/meshes/link.d.dae
new file mode 100644
index 000000000..3716bf02a
--- /dev/null
+++ b/urdf/meshes/link.d.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c1c0a38fa9857a76eae2f4eadbd78f0c56e49eb8ee61e4bceb3470b999b6ca4b
+size 1219217
diff --git a/urdf/meshes/link.e.dae b/urdf/meshes/link.e.dae
new file mode 100644
index 000000000..fe9dfbc0c
--- /dev/null
+++ b/urdf/meshes/link.e.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:90ac9bb7d464ca899e8022218b8a647fb5030a63b06d1afeacfe9aba4a7ad106
+size 3614763
diff --git a/rover_description/meshes/plastic_base.png b/urdf/meshes/plastic_base.png
similarity index 100%
rename from rover_description/meshes/plastic_base.png
rename to urdf/meshes/plastic_base.png
diff --git a/urdf/meshes/rover.dae b/urdf/meshes/rover.dae
new file mode 100644
index 000000000..37eab5984
--- /dev/null
+++ b/urdf/meshes/rover.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0527a3616ec747ff2f9c7aaf9c3ad111be9753d32b3ec8e0f6eff87cbd9a65e3
+size 43685640
diff --git a/urdf/meshes/rover.wheel.dae b/urdf/meshes/rover.wheel.dae
new file mode 100644
index 000000000..0047f9b0a
--- /dev/null
+++ b/urdf/meshes/rover.wheel.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1e8a5fc0a9563d01b92e3fbbb4be5df24383737b4e7e4f3842fc85d21072ee73
+size 6552611
diff --git a/rover_description/meshes/zed2i.dae b/urdf/meshes/zed2i.dae
similarity index 100%
rename from rover_description/meshes/zed2i.dae
rename to urdf/meshes/zed2i.dae
diff --git a/rover_description/rover.xacro b/urdf/rover/rover.urdf.xacro
similarity index 64%
rename from rover_description/rover.xacro
rename to urdf/rover/rover.urdf.xacro
index 91b8f122c..71512c19e 100644
--- a/rover_description/rover.xacro
+++ b/urdf/rover/rover.urdf.xacro
@@ -8,10 +8,9 @@
   <xacro:property name="wheel_width" value="0.1"/>
   <xacro:property name="wheel_mass" value="2"/>
 
-  <!-- <xacro:property name="mesh_scale" value="0.0254 0.0254 0.0254" /> -->
   <xacro:property name="PI" value="3.141592"/>
 
-  <!--Interial macros-->
+  <!-- Inertial macros-->
   <xacro:macro name="cylinder_inertia" params="m r h">
     <inertial>
       <mass value="${m}"/>
@@ -33,14 +32,16 @@
     </inertial>
   </xacro:macro>
 
+  <!-- Links -->
 
-  <!--Base link-->
-  <link name="base_link">
+  <link name="base_link"/>
+
+  <link name="chassis_link">
     <xacro:box_inertia m="3.0" w="${base_len}" h="${base_width}" d="${base_height}"/>
     <visual>
-      <origin rpy="0 0 0" xyz="-0.25 0.0 0.2"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://mrover/rover_description/meshes/rover.dae" scale="1 1 1"/>
+        <mesh filename="package://mrover/urdf/meshes/rover.dae"/>
       </geometry>
     </visual>
     <collision>
@@ -54,9 +55,9 @@
     <link name="front_${side}_wheel_link">
       <xacro:sphere_inertia m="${wheel_mass}" r="${wheel_radius}"/>
       <visual>
-        <origin rpy="${reflect * PI/2} 0 0" xyz="0.0 0.0 0.0"/>
+        <origin rpy="0 ${min(0, reflect) * PI} 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://mrover/rover_description/meshes/wheel.dae" scale="1 1 1"/>
+          <mesh filename="package://mrover/urdf/meshes/rover.wheel.dae"/>
         </geometry>
       </visual>
       <collision>
@@ -69,9 +70,9 @@
     <link name="center_${side}_wheel_link">
       <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
       <visual>
-        <origin rpy="${reflect * PI/2} 0 0" xyz="0.0 0.0 0.0"/>
+        <origin rpy="0 ${min(0, reflect) * PI} 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://mrover/rover_description/meshes/wheel.dae" scale="1 1 1"/>
+          <mesh filename="package://mrover/urdf/meshes/rover.wheel.dae"/>
         </geometry>
       </visual>
       <collision>
@@ -85,9 +86,9 @@
       <xacro:sphere_inertia m="${wheel_mass}" r="${wheel_radius}"/>
 
       <visual>
-        <origin rpy="${reflect * PI/2} 0 0" xyz="0.0 0.0 0.0"/>
+        <origin rpy="0 ${min(0, reflect) * PI} 0" xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://mrover/rover_description/meshes/wheel.dae" scale="1 1 1"/>
+          <mesh filename="package://mrover/urdf/meshes/rover.wheel.dae"/>
         </geometry>
       </visual>
       <collision>
@@ -98,42 +99,48 @@
     </link>
 
     <joint name="front_${side}_wheel_joint" type="continuous">
-      <parent link="base_link"/>
+      <parent link="chassis_link"/>
       <child link="front_${side}_wheel_link"/>
-      <origin xyz="0.42 ${reflect * (base_width/2 + 0.06)} -0.27" rpy="${-PI/2} 0 0"/>
+      <origin xyz="0.42 ${reflect * (base_width/2 + 0.06)} -0.373" rpy="${-PI/2} 0 0"/>
       <axis xyz="0 0 1"/>
     </joint>
 
-
     <joint name="center_${side}_wheel_joint" type="continuous">
-      <parent link="base_link"/>
+      <parent link="chassis_link"/>
       <child link="center_${side}_wheel_link"/>
-      <origin xyz="0 ${reflect * (base_width/2 + 0.06)} -0.27" rpy="${-PI/2} 0 0"/>
+      <origin xyz="0 ${reflect * (base_width/2 + 0.06)} -0.373" rpy="${-PI/2} 0 0"/>
       <axis xyz="0 0 1"/>
     </joint>
 
-
     <joint name="back_${side}_wheel_joint" type="continuous">
-      <parent link="base_link"/>
+      <parent link="chassis_link"/>
       <child link="back_${side}_wheel_link"/>
-      <origin xyz="-0.42 ${reflect*(base_width/2 + 0.06)} -0.27" rpy="${-PI/2} 0 0"/>
+      <origin xyz="-0.42 ${reflect * (base_width/2 + 0.06)} -0.373" rpy="${-PI/2} 0 0"/>
       <axis xyz="0 0 1"/>
     </joint>
   </xacro:macro>
 
+  <!-- Joints -->
+
+  <joint name="base_link_to_chassis" type="fixed">
+    <origin xyz="0 0 0.5"/>
+    <parent link="base_link"/>
+    <child link="chassis_link"/>
+  </joint>
+
   <!-- IMU -->
   <joint name="imu_joint" type="fixed">
     <origin xyz="0 0 0.2"/>
-    <parent link="base_link"/>
+    <parent link="chassis_link"/>
     <child link="imu_link"/>
   </joint>
 
-  <link name="imu_link"></link>
+  <link name="imu_link"/>
 
   <!-- Camera -->
   <joint name="camera_joint" type="fixed">
-    <origin xyz="0 0 0.2"/>
-    <parent link="base_link"/>
+    <origin xyz="0 0 0.1"/>
+    <parent link="chassis_link"/>
     <child link="camera_link"/>
   </joint>
 
@@ -148,7 +155,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://mrover/rover_description/meshes/zed2i.dae" scale="1 1 1"/>
+        <mesh filename="package://mrover/urdf/meshes/zed2i.dae"/>
       </geometry>
     </visual>
 
@@ -159,10 +166,10 @@
     </inertial>
   </link>
 
-  <!-- Left Stereoscopic Camera -->
+  <!-- Left Camera -->
   <joint name="left_camera_joint" type="fixed">
-    <origin xyz="0 0.05 0.2"/>
-    <parent link="base_link"/>
+    <origin xyz="0 -0.05 0.1"/>
+    <parent link="chassis_link"/>
     <child link="left_camera_link"/>
   </joint>
 
@@ -174,42 +181,6 @@
       </geometry>
     </collision>
 
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.06 0.01 0.06"/>
-      </geometry>
-    </visual>
-
-    <inertial>
-      <mass value="1e-5"/>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
-    </inertial>
-  </link>
-
-  <!-- Right Stereoscopic Camera -->
-  <joint name="right_camera_joint" type="fixed">
-    <origin xyz="0 -0.05 0.2"/>
-    <parent link="base_link"/>
-    <child link="right_camera_link"/>
-  </joint>
-
-  <link name="right_camera_link">
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.06 0.01 0.06"/>
-      </geometry>
-    </collision>
-
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.06 0.01 0.06"/>
-      </geometry>
-    </visual>
-
     <inertial>
       <mass value="1e-5"/>
       <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -217,7 +188,7 @@
     </inertial>
   </link>
 
-  <xacro:include filename="$(find mrover)/rover_description/rover_gazebo_plugins.xacro"/>
+  <xacro:include filename="$(find mrover)/urdf/rover/rover_gazebo_plugins.urdf.xacro"/>
   <xacro:wheel side="left" reflect="1"/>
   <xacro:wheel side="right" reflect="-1"/>
 </robot>
diff --git a/rover_description/rover_gazebo_plugins.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro
similarity index 64%
rename from rover_description/rover_gazebo_plugins.xacro
rename to urdf/rover/rover_gazebo_plugins.urdf.xacro
index d1c6a4a7b..5bcf12bd6 100644
--- a/rover_description/rover_gazebo_plugins.xacro
+++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro
@@ -39,7 +39,6 @@
 
   <!-- IMU -->
   <gazebo reference="imu_link">
-    <gravity>false</gravity>
     <sensor name="imu_sensor" type="imu">
       <always_on>true</always_on>
       <update_rate>100</update_rate>
@@ -80,12 +79,12 @@
   <!-- camera -->
   <gazebo reference="camera_link">
     <sensor name="camera_link_camera" type="depth">
-      <update_rate>20</update_rate>
+      <update_rate>15</update_rate>
       <camera>
         <horizontal_fov>1.919862</horizontal_fov>
         <image>
-          <width>640</width>
-          <height>480</height>
+          <width>320</width>
+          <height>240</height>
           <format>R8G8B8</format>
         </image>
         <clip>
@@ -94,16 +93,16 @@
         </clip>
       </camera>
       <plugin name="gazebo_ros_openni_kinect_plugin" filename="libkinect_plugin.so">
-        <baseline>0.2</baseline>
+        <baseline>0</baseline> <!-- Publish same rate as camera -->
         <alwaysOn>true</alwaysOn>
-        <updateRate>1.0</updateRate>
+        <updateRate>5</updateRate>
         <cameraName>camera/left</cameraName>
         <imageTopicName>image</imageTopicName>
         <cameraInfoTopicName>camera_info</cameraInfoTopicName>
         <depthImageTopicName>depth_image</depthImageTopicName>
         <depthImageInfoTopicName>camera_info</depthImageInfoTopicName>
         <pointCloudTopicName>points</pointCloudTopicName>
-        <frameName>camera_link</frameName>
+        <frameName>left_camera_link</frameName>
         <pointCloudCutoff>0.3</pointCloudCutoff>
         <pointCloudCutoffMax>20.0</pointCloudCutoffMax>
         <distortionK1>0.00000001</distortionK1>
@@ -119,68 +118,4 @@
       </plugin>
     </sensor>
   </gazebo>
-
-  <gazebo reference="left_camera_link">
-    <sensor name="left_camera_link_camera" type="camera">
-      <update_rate>20</update_rate>
-      <camera>
-        <horizontal_fov>1.919862</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.2</near>
-          <far>20</far>
-        </clip>
-      </camera>
-      <plugin name="left_camera_controller" filename="libgazebo_ros_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>20</updateRate>
-        <cameraName>camera/left</cameraName>
-        <imageTopicName>image</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>base_link</frameName>
-        <hackBaseline>0.02</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-      </plugin>
-    </sensor>
-  </gazebo>
-
-  <gazebo reference="right_camera_link">
-    <sensor name="right_camera_link_camera" type="camera">
-      <update_rate>20</update_rate>
-      <camera>
-        <horizontal_fov>1.919862</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.2</near>
-          <far>20</far>
-        </clip>
-      </camera>
-      <plugin name="right_camera_controller" filename="libgazebo_ros_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>20</updateRate>
-        <cameraName>camera/right</cameraName>
-        <imageTopicName>image</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>base_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>
 </robot>
diff --git a/urdf/staging/arm.blend b/urdf/staging/arm.blend
new file mode 100644
index 000000000..3ef704199
--- /dev/null
+++ b/urdf/staging/arm.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6c2db5502473b9f958e4981795b9d71cbfaa9a6fd2229c5020512c970248afa
+size 21306896
diff --git a/urdf/staging/arm.dae.blend b/urdf/staging/arm.dae.blend
new file mode 100644
index 000000000..09a3fe6e5
--- /dev/null
+++ b/urdf/staging/arm.dae.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bef57fee62da0bb992ce8abc8f99d8f14d2957b9a4c899ace10dc0a19f3a8eba
+size 932168
diff --git a/rover_description/meshes/artag.blend b/urdf/staging/artag.blend
similarity index 100%
rename from rover_description/meshes/artag.blend
rename to urdf/staging/artag.blend
diff --git a/urdf/staging/rover.blend b/urdf/staging/rover.blend
new file mode 100644
index 000000000..28b6e595a
--- /dev/null
+++ b/urdf/staging/rover.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88f36161c9c15cbd3ea6e6ed5aeedbde44c6a958dfacf0874b4ca2cbe280c89e
+size 24910412
diff --git a/urdf/staging/rover.dae.blend b/urdf/staging/rover.dae.blend
new file mode 100644
index 000000000..9d80cf866
--- /dev/null
+++ b/urdf/staging/rover.dae.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:93d0ca5b5c9d062bca9bca334d876067cbbd9218ac57aa405e8853266098a36c
+size 879440
diff --git a/rover_description/meshes/zed2i.blend b/urdf/staging/zed2i.blend
similarity index 100%
rename from rover_description/meshes/zed2i.blend
rename to urdf/staging/zed2i.blend
diff --git a/rover_description/zed/include/materials.urdf.xacro b/urdf/zed/include/materials.urdf.xacro
similarity index 100%
rename from rover_description/zed/include/materials.urdf.xacro
rename to urdf/zed/include/materials.urdf.xacro
diff --git a/rover_description/zed/zed_descr.urdf.xacro b/urdf/zed/zed_descr.urdf.xacro
similarity index 94%
rename from rover_description/zed/zed_descr.urdf.xacro
rename to urdf/zed/zed_descr.urdf.xacro
index b42391635..cff4fd376 100644
--- a/rover_description/zed/zed_descr.urdf.xacro
+++ b/urdf/zed/zed_descr.urdf.xacro
@@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     <xacro:arg name="cam_pitch" default="0.0"/>
     <xacro:arg name="cam_yaw" default="0.0"/>
 
-    <xacro:include filename="$(find mrover)/rover_description/zed/zed_macro.urdf.xacro"/>
+    <xacro:include filename="$(find mrover)/urdf/zed/zed_macro.urdf.xacro"/>
     <xacro:zed_camera name="$(arg camera_name)" model="$(arg camera_model)" parent="$(arg base_frame)">
         <origin xyz="$(arg cam_pos_x) $(arg cam_pos_y) $(arg cam_pos_z)" rpy="$(arg cam_roll) $(arg cam_pitch) $(arg cam_yaw)"/>
     </xacro:zed_camera>
diff --git a/rover_description/zed/zed_macro.urdf.xacro b/urdf/zed/zed_macro.urdf.xacro
similarity index 95%
rename from rover_description/zed/zed_macro.urdf.xacro
rename to urdf/zed/zed_macro.urdf.xacro
index 7e7dd87af..3c852d45c 100644
--- a/rover_description/zed/zed_macro.urdf.xacro
+++ b/urdf/zed/zed_macro.urdf.xacro
@@ -19,7 +19,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 -->
 
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
-    <xacro:include filename="$(find mrover)/rover_description/zed/include/materials.urdf.xacro"/>
+    <xacro:include filename="$(find mrover)/urdf/zed/include/materials.urdf.xacro"/>
     <xacro:property name="M_PI" value="3.1415926535897931"/>
 
     <!--
@@ -74,13 +74,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
             <visual>
                 <origin xyz="${screw_offset_x} 0 0" rpy="0 0 0"/>
                 <geometry>
-                    <mesh filename="package://mrover/rover_description/meshes/${model}.dae"/>
+                    <mesh filename="package://mrover/urdf/meshes/${model}.dae"/>
                 </geometry>
                 <material name="${model}_mat"/>
             </visual>
             <collision>
                 <geometry>
-                    <mesh filename="package://mrover/rover_description/meshes/${model}.dae"/>
+                    <mesh filename="package://mrover/urdf/meshes/${model}.dae"/>
                 </geometry>
             </collision>
         </link>