diff --git a/cv/lane_detection/config/lane_detection_params.yaml b/cv/lane_detection/config/lane_detection_params.yaml
index 5d728ac..3b2196d 100644
--- a/cv/lane_detection/config/lane_detection_params.yaml
+++ b/cv/lane_detection/config/lane_detection_params.yaml
@@ -1 +1 @@
-lane_detection_mode: 1 # 0 => DEEP LEARNING, 1 => CLASSICAL
\ No newline at end of file
+lane_detection_mode: 0 # 0 => DEEP LEARNING, 1 => CLASSICAL
\ No newline at end of file
diff --git a/cv/lane_detection/config/num.npy b/cv/lane_detection/config/num.npy
new file mode 100644
index 0000000..f833740
Binary files /dev/null and b/cv/lane_detection/config/num.npy differ
diff --git a/cv/lane_detection/launch/lane_detection_server.launch b/cv/lane_detection/launch/lane_detection_server.launch
index 65166a7..d1f0a40 100644
--- a/cv/lane_detection/launch/lane_detection_server.launch
+++ b/cv/lane_detection/launch/lane_detection_server.launch
@@ -24,13 +24,13 @@
     </node> -->
 
     
-    <!-- <node
+    <node
         name="lane_viz"
         pkg="lane_detection"
         type="lane_viz.py"
         output="screen"
     >
-    </node> -->
+    </node>
 
     <node
         name="lane_scan_cpp"
diff --git a/cv/lane_detection/src/depth_sensing.py b/cv/lane_detection/src/depth_sensing.py
new file mode 100644
index 0000000..4293b00
--- /dev/null
+++ b/cv/lane_detection/src/depth_sensing.py
@@ -0,0 +1,66 @@
+########################################################################
+#
+# Copyright (c) 2022, STEREOLABS.
+#
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+########################################################################
+
+import pyzed.sl as sl
+import math
+import numpy as np
+import sys
+import math
+
+def main():
+    # Create a Camera object
+    zed = sl.Camera()
+
+    # Create a InitParameters object and set configuration parameters
+    init_params = sl.InitParameters(depth_mode=sl.DEPTH_MODE.NEURAL,
+                                    coordinate_units=sl.UNIT.METER,
+                                    coordinate_system=sl.COORDINATE_SYSTEM.IMAGE)
+
+    # Open the camera
+    status = zed.open(init_params)
+    if status != sl.ERROR_CODE.SUCCESS: #Ensure the camera has opened succesfully
+        print("Camera Can't Open.... "+repr(status)+". Exit program.")
+        exit()
+
+    # Create and set RuntimeParameters after opening the camera
+    runtime_parameters = sl.RuntimeParameters()
+    
+    point_cloud = sl.Mat()
+
+    mirror_ref = sl.Transform()
+    mirror_ref.set_translation(sl.Translation(2.75,4.0,0))
+
+    # A new image is available if grab() returns SUCCESS
+    if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
+        # Retrieve colored point cloud. Point cloud is aligned on the left image.
+        zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
+
+    np.save('/home/tsyh/Documents/num',point_cloud.get_data()[:, :, 0:3])
+    err = point_cloud.write('/home/tsyh/Documents/point.xyz')
+    if(err == sl.ERROR_CODE.SUCCESS):
+        print("point cloud saved")
+    else:
+        print("the point cloud has not been saved")
+
+    # Close the camera
+    zed.close()
+
+if __name__ == "__main__":
+    main()
diff --git a/cv/lane_detection/src/lane_detection_inference.py b/cv/lane_detection/src/lane_detection_inference.py
index 822d805..18d0af8 100755
--- a/cv/lane_detection/src/lane_detection_inference.py
+++ b/cv/lane_detection/src/lane_detection_inference.py
@@ -10,6 +10,7 @@
 import onnx
 import onnxruntime as ort 
 import torch
+import pandas as pd
 
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
@@ -23,13 +24,13 @@
 from cv_utils import camera_projection
 
 from line_fitting import fit_lanes
-from unet_lane.Inference import Inference
 from ultralytics import YOLO
 
 from threshold_lane.threshold import lane_detection
 
 # import open3d as o3d
-from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import CameraInfo, LaserScan, PointCloud2, PointField
+from sensor_msgs import point_cloud2
 
 
 class CVModelInferencer:
@@ -38,25 +39,21 @@ def __init__(self):
         
         self.pub = rospy.Publisher('cv/lane_detections', FloatArray, queue_size=10)
         self.pub_raw = rospy.Publisher('cv/model_output', Image, queue_size=10)
+        self.pub_pt = rospy.Publisher('cv/lane_detections_cloud', PointCloud2, queue_size=10)
+        # self.pub_scan = rospy.Publisher('cv/lane_detections_scan', LaserScan, queue_size=10)
 
         self.bridge = CvBridge()
         self.projection = camera_projection.CameraProjection()
         
         rospack = rospkg.RosPack()
-        # self.model_path = rospack.get_path('lane_detection') + '/models/competition_model_4c_128.pt'
         self.model_path = rospack.get_path('lane_detection') + '/models/best.pt'
+        self.depth_map_path = rospack.get_path('lane_detection') + '/config/num.npy'
+
 
         # Get the parameter to decide between deep learning and classical
         self.classical_mode = rospy.get_param('/lane_detection_inference/lane_detection_mode')
         self.Inference = None
         self.lane_detection = None
-        
-        # Load in the depth matrix
-        # depth_dir = rospack.get_path('lane_detection') + '/config/depth_sim.npy'
-        # depth_matrix_np = np.load(depth_dir)
-        # depth_matrix_np = depth_matrix_np * 1000 # Convert from (m) to (mm)
-        # depth_matrix_np = cv2.resize(depth_matrix_np, (330, 180))
-        # self.depth_matrix = o3d.geometry.Image(depth_matrix_np.astype(np.float32))
     
         if self.classical_mode == 1:
             self.lane_detection = lane_detection
@@ -67,10 +64,10 @@ def __init__(self):
 
             rospy.loginfo("Lane Detection node initialized with DEEP LEARNING...\nCUDA status: %s ", torch.cuda.is_available())
 
-        # self.hack = cv2.imread(r'/home/ammarvora/utra/espresso-ws/src/Espresso/cv/lane_detection/src/lane.png')
-
-        # print(self.hack.shape)
-
+        # listen for transform from camera to lidar frames
+        # self.listener = tf.TransformListener()
+        # self.listener.waitForTransform("/left_camera_link_optical", "/base_laser", rospy.Time(), rospy.Duration(10.0))
+        
 
         
     def run(self):
@@ -93,9 +90,6 @@ def lane_transform(self, img):
                                 [int(width/2)-new_width,length]])
         M2 = cv2.getPerspectiveTransform(input_pts,output_pts)
         out = cv2.warpPerspective(img,M2,(width, length),flags=cv2.INTER_LINEAR)
-        # plt.imshow(out)
-        # cv2.imshow('test', out)
-        # cv2.waitKey(0)
         return out
 
 
@@ -104,15 +98,14 @@ def process_image(self, data):
             return
             
         raw = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
+        projected_lanes = np.load(self.depth_map_path)
+
         
         if raw is not None:
             # Get the image
             input_img = raw.copy()
-            # input_img = self.hack
-            
-            # input_img = cv2.resize(raw.shape[1], raw.shape[0])
+            input_img = cv2.resize(input_img, (330, 180))
             
-            # cv2.imwrite(r'/home/ammarvora/utra/espresso-ws/src/Espresso/cv/lane_detection/src' + 'frame.png', input_img)
             # Do model inference 
             output = None
             mask = None
@@ -122,48 +115,29 @@ def process_image(self, data):
 
                 mask = np.where(output > 0.5, 1., 0.)
                 mask = mask.astype(np.uint8)
-                mask = cv2.resize(mask, (330, 180))
 
             else:
                 # output = self.Inference.inference(input_img)
-                # input_img = cv2.resize(input_img, (330, 180))
-                cv2.rectangle(input_img, (0,0), (input_img.shape[1],int(input_img.shape[0] / 10)), (0,0,0), -1) 
-                # cv2.imwrite(r'/home/tsyh/Downloads/test.jpg', input_img.squeeze())
+                # cv2.rectangle(input_img, (0,0), (input_img.shape[1],int(input_img.shape[0] / 9)), (0,0,0), -1) 
             
                 output = self.Inference(input_img)
                 confidence_threshold = 0.5
-                # number_masks = sum(1 for box in results[0].boxes if float(box.conf) > confidence_threshold)
-                # print("number masks: ", number_masks)
 
-                labels = {}
                 output_image = np.zeros_like(input_img[:,:,0], dtype=np.uint8)
+                # output_image = np.zeros_like(projected_lanes[:,:,0], dtype=np.uint8)
 
                 if output[0].masks:
                     for k in range(len(output[0].masks)):
                         mask = np.array(output[0].masks[k].data.cpu() if torch.cuda.is_available() else output[0].masks[k].data)  # Convert tensor to numpy array
                         label = output[0].names[int(output[0].boxes[k].cls)]
 
-                        if label not in labels:
-                            labels[label] = np.zeros((180,330), dtype=np.uint8)
-
                         if float(output[0].boxes[k].conf) > confidence_threshold:  # Check confidence level
                             if label == 'lane':
                                 img = np.where(mask > 0.5, 255, 0).astype(np.uint8)
                                 img = cv2.resize(img.squeeze(), (output_image.shape[1], output_image.shape[0]))
                                 output_image = np.maximum(output_image, img)
 
-                            resize_mask = np.where(mask > 0.5, 1., 0.).astype(np.uint8)
-                            resize_mask = cv2.resize(resize_mask.squeeze(), (330, 180))
-
-                            labels[label] = np.maximum(labels[label], resize_mask)
                 output = output_image
-                mask = labels['lane'] if 'lane' in labels else np.zeros((330,180), dtype=np.uint8)
-
-
-
-            # mask = np.where(output > 0.5, 1., 0.)
-            # mask = mask.astype(np.uint8)
-            # mask = cv2.resize(mask, (330, 180))
 
             # Publish to /cv/model_output
             img_msg = self.bridge.cv2_to_imgmsg(output, encoding='passthrough')
@@ -172,32 +146,22 @@ def process_image(self, data):
             if img_msg is not None:
                 self.pub_raw.publish(img_msg)
             
-
-            '''The following code is needed for virtual layers'''
-            rows = np.where(mask==1)[0].reshape(-1,1)
-            cols = np.where(mask==1)[1].reshape(-1,1)
-            lane_table = np.concatenate((cols,rows),axis=1)
-
-            # print(lane_table)
-
-            # ta = time.time()
-            projected_lanes = self.projection(lane_table)
-            # tb = time.time()
-
-            # print(f'PROJECTION FPS: {1 / (tb - ta)}')
-
             # Build the message
             lane_msg = FloatList()
             pts_msg = []
+            cloud = []
 
-            for pt in projected_lanes:
+            for i in range(output.shape[0]):
+                for j in range(output.shape[1]):
+                    if((output[i][j])==255):
+                        pt_msg = Point()
+                        pt_msg.x = projected_lanes[i][j][0]
+                        pt_msg.y = projected_lanes[i][j][1]
+                        pt_msg.z = projected_lanes[i][j][2]
 
-                pt_msg = Point()
-                pt_msg.x = pt[0]
-                pt_msg.y = pt[1]
-                pt_msg.z = pt[2]
+                        pts_msg.append(pt_msg)
+                        cloud.append(projected_lanes[i][j])
 
-                pts_msg.append(pt_msg)
             lane_msg.elements = pts_msg
 
 
@@ -206,6 +170,11 @@ def process_image(self, data):
             msg.header.stamp = data.header.stamp
             self.pub.publish(msg)
 
+            pt_header = Header(frame_id='left_camera_link_optical')
+            pt_header.stamp = data.header.stamp
+            pt_cloud = point_cloud2.create_cloud_xyz32(header=pt_header, points=cloud)
+            self.pub_pt.publish(pt_cloud)
+    
                 
 
 
diff --git a/cv/lane_detection/src/lane_viz.py b/cv/lane_detection/src/lane_viz.py
index fbd722f..10ca399 100755
--- a/cv/lane_detection/src/lane_viz.py
+++ b/cv/lane_detection/src/lane_viz.py
@@ -19,7 +19,7 @@ class FloatArrayToPointCloud2Node:
     def __init__(self):
         rospy.init_node("float_array_to_pc2_node")
         self.points2_pub = rospy.Publisher(
-            "/cv/lane_detections_cloud", PointCloud2, queue_size=1
+            "/cv/lane_cloud", PointCloud2, queue_size=1
         )
 
         # listen for transform from camera to lidar frames
diff --git a/description/config/cartographer.lua b/description/config/cartographer.lua
index 816f7a9..4661daa 100644
--- a/description/config/cartographer.lua
+++ b/description/config/cartographer.lua
@@ -35,10 +35,10 @@ end
 
 -- check if cv published
 local topic_name = "/cv/lane_detections\n" -- DON'T REMOVE THE SPACE AFTER. WE NEED IT
-num_scan = 1
+num_scan = 0
 if is_topic_published(topic_name) then
   print("......................cv published")
-    num_scan = 2
+    num_scan = 1
 end
 
 print("NUMBER SCAN: ", num_scan)
@@ -47,7 +47,7 @@ options = {
   map_builder = MAP_BUILDER,
   trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
-  tracking_frame = "imu_link",
+  tracking_frame = "base_link",
   published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
@@ -56,10 +56,10 @@ options = {
   use_odometry = true,
   use_nav_sat = true,
   use_landmarks = false,
-  num_laser_scans = num_scan,
+  num_laser_scans = 1-num_scan,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
-  num_point_clouds = 0,
+  num_point_clouds = num_scan,
   lookup_transform_timeout_sec = 1.,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
@@ -76,8 +76,8 @@ MAP_BUILDER.use_trajectory_builder_2d = true
 TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
 
 TRAJECTORY_BUILDER_2D.min_range = 0.1
-TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
-TRAJECTORY_BUILDER_2D.use_imu_data = true
+TRAJECTORY_BUILDER_2D.missing_data_ray_length = 15.
+TRAJECTORY_BUILDER_2D.use_imu_data = false
 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10
 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 15
 
@@ -97,7 +97,7 @@ POSE_GRAPH.global_constraint_search_after_n_seconds = 30 -- Increase
 
 ---------Global/Local SLAM---------
 TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100 -- Decrease
-TRAJECTORY_BUILDER_2D.max_range = 3.5 -- Decrease
+TRAJECTORY_BUILDER_2D.max_range = 10 -- Decrease
 
 -------------------------------------------------------------------------------------
 
diff --git a/description/launch/cartographer.launch b/description/launch/cartographer.launch
index 4442561..80a1835 100644
--- a/description/launch/cartographer.launch
+++ b/description/launch/cartographer.launch
@@ -8,11 +8,11 @@
     <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="-configuration_directory $(find description)/config -configuration_basename cartographer.lua" output="screen">
 
         <remap from="imu" to="/imu/data" />
-        <remap from="odom" to="/odom" />
+        <remap from="odom" to="/wheel_odom/euler" />
         <remap from="scan" to="/scan_modified" />
-        <remap from="scan_1" to="/cv/lane_detections_scan"/>
-        <remap from="scan_2" to="/scan_modified" />
-        <!-- <remap from="points2" to="/cv/lane_detections_cloud" />  -->
+        <!-- <remap from="scan_1" to="/cv/lane_detections_scan"/>
+        <remap from="scan_2" to="/scan_modified" /> -->
+        <remap from="points2" to="/cv/lane_detections_cloud" /> 
         <remap from="fix" to="/gps/fix"/>
     </node>
 
diff --git a/description/launch/simulate.launch b/description/launch/simulate.launch
index 911fbca..98c8242 100644
--- a/description/launch/simulate.launch
+++ b/description/launch/simulate.launch
@@ -6,7 +6,7 @@
 
     <!-- IGVC worlds: full, walls, ramp, plain -->
     <!-- IGVC world types: pavement (2022 IGVC) -->
-    <arg name="world" default="full"/>
+    <arg name="world" default="ramp"/>
     <arg name="world_type" default="pavement"/>
 
     <!-- Simulate 'world' in Gazebo -->
@@ -44,6 +44,6 @@
     args="-19.5 0 0 1.5707 0 0 world ground_truth" />
 
     <!-- Cartographer (Mapping), publishes odom and map frame transforms -->
-    <include file="$(find description)/launch/cartographer.launch" />
+    <!-- <include file="$(find description)/launch/cartographer.launch" /> -->
 
 </launch>
diff --git a/description/src/poor_mans_scheduler.py b/description/src/poor_mans_scheduler.py
index 45bb8f2..5aed89e 100644
--- a/description/src/poor_mans_scheduler.py
+++ b/description/src/poor_mans_scheduler.py
@@ -58,7 +58,7 @@ def __init__(self):
         self.merged_scan_set = False
         self.assign_topic('/pause_navigation', 'manual_default_set', Bool, True)
         self.assign_topic('/scan_modified', 'scan_override_set', LaserScan)
-        #self.assign_topic('/cv/lane_detections_cloud','cv_cloud_set',PointCloud2)
+        #self.assign_topic('/cv/lane_cloud','cv_cloud_set',PointCloud2)
         self.assign_topic('/cv/lane_detections_scan','cv_lane_scan_set',LaserScan)
         self.assign_topic('/scan_merged','merged_scan_set',LaserScan)
 
diff --git a/description/src/scheduler.py b/description/src/scheduler.py
old mode 100644
new mode 100755
index f47d66d..d2f8c58
--- a/description/src/scheduler.py
+++ b/description/src/scheduler.py
@@ -1,7 +1,6 @@
 #!/usr/bin/python3
 import os
 import time
-import paramiko #need to install paramiko Python package
 
 import rospy
 import tf
@@ -44,7 +43,7 @@ def __init__(self):
         self.assign_topic('/odometry/global', 'odom_global_published', Odometry)
         self.assign_topic('/odometry/gps', 'odom_gps_published', Odometry)
         self.assign_topic('/odometry/local', 'odom_local_published', Odometry)
-        self.assign_topic('/fake_odom_wheel_encoder_quat', 'odom_motor_published', Odometry)
+        self.assign_topic('/wheel_odom/euler', 'odom_motor_published', Odometry)
 
         #Cartographer
         self.tracked_pose_set = False
@@ -58,15 +57,13 @@ def __init__(self):
         self.merged_scan_set = False
         self.assign_topic('/pause_navigation', 'manual_default_set', Bool, True)
         self.assign_topic('/scan_modified', 'scan_override_set', LaserScan)
-        #self.assign_topic('/cv/lane_detections_cloud','cv_cloud_set',PointCloud2)
+        #self.assign_topic('/cv/lane_cloud','cv_cloud_set',PointCloud2)
         self.assign_topic('/cv/lane_detections_scan','cv_lane_scan_set',LaserScan)
         self.assign_topic('/scan_merged','merged_scan_set',LaserScan)
 
         #SSH
         self.raspberry_pi2 = "10.0.0.2" #IP Address
         self.raspberry_pi3 = "10.0.0.3" #IP Address
-        self.username = "ubuntu"
-        self.password = "utraart2021"
     
     def abort(self, msg):
         rospy.logerr("SOMETHING FAILED. ABORTING. THIS CAUSED IT:" + msg)
@@ -165,20 +162,16 @@ def run(self):
 
         # Launch sensors, wait for success
         rospy.loginfo('Starting up sensors...')
-        os.system('roslaunch sensors sensors.launch launch_state:=IGVC frame_id:=gps_link &> /dev/null &')
+        # os.system('roslaunch sensors sensors.launch launch_state:=IGVC frame_id:=gps_link &> /dev/null &') # Lidar has trouble launching, do it separately
+        os.system('roslaunch phidgets_imu imu.launch &> /dev/null &')
+        os.system('roslaunch nmea_navsat_driver nmea_serial_driver.launch &> /dev/null &')
         self.wait_for_condition('imu_started', 35)
         self.wait_for_condition('lower_lidar_started', 35)
         self.wait_for_condition('upper_lidar_started', 40)
         self.wait_for_condition('gps_started', 90)
         rospy.loginfo('Sensors launched.')
 
-        # override scan 
-        rospy.loginfo('Launching scan override...')
-        os.system('roslaunch filter_lidar_data filter_lidar_data.launch &> /dev/null &')
-        self.wait_for_condition('scan_override_set', 20)
-        rospy.loginfo('Scan overriden.')
-
-        #run zed
+        # run zed
         rospy.loginfo('Start ZED separately...')
         if rospy.get_param('/scheduler/visual_odom_enable'):
             os.system('roslaunch zed_wrapper zed_no_tf.launch position_tracking:=true &> /dev/null &')
@@ -190,53 +183,37 @@ def run(self):
         # Run cv pipeline, wait for zed 
         rospy.loginfo('Starting CV pipeline...')
         os.system('roslaunch cv cv_pipeline.launch launch_state:=IGVC &> /dev/null &')
-        #self.wait_for_condition('zed_started', 35)
         #self.wait_for_condition('cv_cloud_set', 30)
         self.wait_for_condition('cv_lane_scan_set', 70)
-        self.wait_for_condition('merged_scan_set', 100)
+        # self.wait_for_condition('merged_scan_set', 100)
         rospy.loginfo('CV pipeline launched.')
-        
-        # # Run motor control, teleop and feedback, wait for /odom 
-        # rospy.loginfo('Starting motor controls...')
-        # self.initiate_ssh(self.raspberry_pi3, self.username, self.password)
-
-        # _stdin, _stdout, _stderr = self.ssh_client.exec_command('cd ~/espresso-ws && source devel/setup.bash && roslaunch motor_control teleop_motor_control.launch')
-        # #print(_stdout.read().decode()) #prints the stdout of the command
-
-        # #os.system('roslaunch description motor_control_pipeline.launch launch_state:=IGVC &> /dev/null &')
-        # # self.wait_for_transform(listener, 'base_link', 'odom')
-        # # self.wait_for_condition('odom_motor_published', 30)
-        # rospy.loginfo('Motor controls started.')
-        # #self.close_ssh()
-
-        #Run motor_odom_node
-        rospy.loginfo('Starting motor_odom_node...')
-        os.system('roslaunch motor_odom motor_odom.launch launch_state:=IGVC &> /dev/null &')
+
+        # Check motor_odom_node (Done on PI, don't launch here)
+        rospy.loginfo('Checking motor_odom_node...')
         self.wait_for_condition('odom_motor_published', 50)
         rospy.loginfo('motor_odom_node launched.')
 
         # Run odom, wait for odom local, global
-        rospy.loginfo('Initializing odometry...')
-        #self.initiate_ssh(self.raspberry_pi2, self.username, self.password)
-
-        #_stdin, _stdout, _stderr = self.ssh_client.exec_command('cd ~/espresso-ws && source devel/setup.bash && roslaunch odom odom.launch launch_state:=IGVC &> /dev/null &')
-        #print(_stdout.read().decode()) #prints out the stdout of the command
-
+        rospy.loginfo('Initializing other odometry...')
         os.system('roslaunch odom odom.launch launch_state:=IGVC &> /dev/null &')
         self.wait_for_condition('odom_global_published', 35)
         self.wait_for_condition('odom_local_published', 35)
+        rospy.loginfo('Odometry (local/global) initialized.')
 
-        rospy.loginfo('Odometry initialized.')
-        #self.close_ssh()
-
-        # Run utm frame transform,wait for odometry/gps
+        # Run utm frame transform, wait for odometry/gps
         rospy.loginfo('Initializing UTM...')
         os.system('roslaunch description utm.launch &> /dev/null &')
         self.wait_for_transform(listener, '/map', '/utm', timeout = 120)
         self.wait_for_condition('odom_gps_published', 45)
         rospy.loginfo('UTM initialized.')
 
-        #Run cartographer
+        # override scan 
+        rospy.loginfo('Launching scan override...')
+        os.system('roslaunch filter_lidar_data filter_lidar_data.launch launch_state:=IGVC &> /dev/null &')
+        self.wait_for_condition('scan_override_set', 20)
+        rospy.loginfo('Scan overriden.')
+
+        # Run cartographer
         rospy.loginfo('Starting cartographer...')
         os.system('roslaunch description cartographer.launch launch_state:=IGVC &> /dev/null &')
         self.wait_for_condition('tracked_pose_set', 60)
@@ -248,20 +225,23 @@ def run(self):
         rospy.loginfo('Initializing navigation stack...')
         os.system('roslaunch nav_stack move_base.launch launch_state:=IGVC &> /dev/null &')
         self.wait_for_transform(listener, '/base_link', '/map')
-        #self.wait_for_transform(listener, '/map', '/utm', timeout=120)
         rospy.loginfo('Navstack initialized.')
-        #rospy.loginfo('UTM initialized.')
 
-        ## teleop 
+        ## teleop (Launched on PI, don't launch)
         ## rospy.loginfo('Starting teleop...')
         ## os.system('roslaunch teleop_twist_keyboard keyboard_teleop.launch')
         ## rospy.loginfo('Teleop launched.')
 
-        # # load waypoints 
+        # load waypoints 
         # rospy.loginfo('Loading waypoints...')
         # os.system('roslaunch load_waypoints load_waypoints.launch launch_state:=IGVC &> /dev/null &')
         # rospy.loginfo('Waypoints loaded.') 
 
+        # Rosservice for relative and single gps goals
+        rospy.loginfo('Loading waypoints...')
+        os.system('roslaunch load_waypoints nav_options.launch &> /dev/null &')
+        rospy.loginfo('Nav options loaded.') 
+        
         #rviz
         rospy.loginfo('Starting rviz...')
         os.system('roslaunch description rviz.launch &> /dev/null &')
@@ -275,6 +255,5 @@ def run(self):
     if scheduler.run():
         rospy.loginfo("Scheduler succeeded. Espresso is ready to rumble!")
         scheduler.unsubscribe_all()
-        # scheduler.close_ssh()
         while not rospy.is_shutdown():
             pass
diff --git a/nav/load_waypoints/scripts/IGVC_course.json b/nav/load_waypoints/scripts/IGVC_course.json
index fbbad7d..110a4c7 100644
--- a/nav/load_waypoints/scripts/IGVC_course.json
+++ b/nav/load_waypoints/scripts/IGVC_course.json
@@ -1,118 +1,35 @@
 {
     "start_direction": "north",
-    "laps": 2,
+    "laps": 1,
     "add_corners": false,
+    "add_manual_points": false,
     "waypoints" : [
         {
             "id" : 0,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "First corner",
+            "longitude" : -83.21934030,
+            "latitude" : 42.66826972,
+            "description"  : "North waypoint",
             "frame_id" : "map"
         },
         {
             "id" : 1,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Second corner",
+            "longitude" : -83.21936061,
+            "latitude" : 42.66812064,
+            "description"  : "Ramp start waypoint",
             "frame_id" : "map"
         },
         {
             "id" : 2,
-            "longitude" : -83.130635756,
-            "latitude" : 42.400465621,
-            "description"  : "Start of no mans",
+            "longitude" : -83.21935916,
+            "latitude" : 42.66807663,
+            "description"  : "Ramp end waypoint",
             "frame_id" : "map"
         },
         {
             "id" : 3,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "Ramp entrance",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 4,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Ramp exit",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 5,
-            "longitude" : -83.130635756,
-            "latitude" : 42.400465621,
-            "description"  : "End of no mans",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 6,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "Third corner",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 7,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Forth corner",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 8,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "First corner",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 9,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Second corner",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 10,
-            "longitude" : -83.130635756,
-            "latitude" : 42.400465621,
-            "description"  : "Start of no mans",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 11,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "Ramp entrance",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 12,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Ramp exit",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 13,
-            "longitude" : -83.130635756,
-            "latitude" : 42.400465621,
-            "description"  : "End of no mans",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 14,
-            "longitude" : -83.130645144,
-            "latitude" : 42.400556255,
-            "description"  : "Third corner",
-            "frame_id" : "map"
-        },
-        {
-            "id" : 15,
-            "longitude" : -83.130640432,
-            "latitude" : 42.400510946,
-            "description"  : "Forth corner",
+            "longitude" : -83.21932764,
+            "latitude" : 42.66792771,
+            "description"  : "South waypoint",
             "frame_id" : "map"
         }
     ]
diff --git a/nav/load_waypoints/scripts/IGVC_practice.json b/nav/load_waypoints/scripts/IGVC_practice.json
index f8d80a5..01772ac 100644
--- a/nav/load_waypoints/scripts/IGVC_practice.json
+++ b/nav/load_waypoints/scripts/IGVC_practice.json
@@ -6,10 +6,24 @@
     "waypoints" : [
         {
             "id" : 0,
-            "longitude" : -83.218459,
-            "latitude" : 42.668212,
+            "longitude" : -83.21845873,
+            "latitude" : 42.66821182,
             "description"  : "North waypoint",
             "frame_id" : "map"
+        },
+        {
+            "id" : 1,
+            "longitude" : -83.21844564,
+            "latitude" : 42.66808596,
+            "description"  : "Mid waypoint",
+            "frame_id" : "map"
+        },
+        {
+            "id" : 2,
+            "longitude" : -83.21843266,
+            "latitude" : 42.66796006,
+            "description"  : "South waypoint",
+            "frame_id" : "map"
         }
     ]
 }
diff --git a/nav/nav_stack/config/costmap_common_params.yaml b/nav/nav_stack/config/costmap_common_params.yaml
index 9ecd48d..10730fd 100644
--- a/nav/nav_stack/config/costmap_common_params.yaml
+++ b/nav/nav_stack/config/costmap_common_params.yaml
@@ -1,4 +1,4 @@
-max_obstacle_height: 3.3 # Can be a large number, currently matched to the obstacle range
+max_obstacle_height: 3.3 # Can be a large number, currently matched to the obstacle range  (x up, y left)
 footprint: [[-0.49, -0.46], [-0.49, 0.46], [0.45, 0.46], [0.59, 0.10], [0.59, -0.10], [0.45, -0.46]]
 
 # NOTE: Espresso is generally parallel to the ground with 3-wheel drive
diff --git a/nav/nav_stack/config/global_costmap_params.yaml b/nav/nav_stack/config/global_costmap_params.yaml
index 58b6974..175fd42 100644
--- a/nav/nav_stack/config/global_costmap_params.yaml
+++ b/nav/nav_stack/config/global_costmap_params.yaml
@@ -61,7 +61,7 @@ global_costmap:
   # Inflation layer is necessary to put an area of medium cost around the obstacles (cartographer does not do this)
   inflation_layer:
     enabled:              true
-    inflation_radius:     1.0 # Tunable parameter (affect navigation plan)
+    inflation_radius:     0.75 # Tunable parameter (affect navigation plan)
     cost_scaling_factor: 2.0
 
       # plugins:
diff --git a/nav/nav_stack/config/local_costmap_params.yaml b/nav/nav_stack/config/local_costmap_params.yaml
index be43dd7..f5c3fbe 100644
--- a/nav/nav_stack/config/local_costmap_params.yaml
+++ b/nav/nav_stack/config/local_costmap_params.yaml
@@ -7,8 +7,8 @@ local_costmap:
   rolling_window: true
   always_send_full_costmap: false
   # TODO: Update size if needed
-  width: 15 # cost map size needs to be large enough to encompass the point cloud range
-  height: 15 # This size is arbitary for now so it may need to be adjusted
+  width: 10 # cost map size needs to be large enough to encompass the point cloud range
+  height: 10 # This size is arbitary for now so it may need to be adjusted
   #resolution: 0.02
 
   obstacle_layer:
@@ -57,7 +57,7 @@ local_costmap:
 
   inflation_layer:
     enabled:              true
-    inflation_radius:     1.0  # Tunable parameter (affect navigation plan)
+    inflation_radius:     0.75  # Tunable parameter (affect navigation plan)
     cost_scaling_factor:    2.0
 
   plugins:
diff --git a/nav/nav_stack/config/local_global_planner.yaml b/nav/nav_stack/config/local_global_planner.yaml
index 176c188..72bf2ba 100644
--- a/nav/nav_stack/config/local_global_planner.yaml
+++ b/nav/nav_stack/config/local_global_planner.yaml
@@ -6,9 +6,10 @@ GlobalPlanner:
   default_tolerance: 0.1
  # use_dijkstra: false
   visualize_potential: true
+  old_navfn_behavior: true
   #use_grid_path: true
-  cost_factor: 0.55
-  neutral_cost: 66
+  cost_factor: 0.8
+  neutral_cost: 50
   lethal_cost: 253
 
 # Navigation Function (Backup Planner)
@@ -20,6 +21,29 @@ NavfnROS: #http://wiki.ros.org/navfn
 
 ##### Local Planner #####
 
+TebLocalPlannerROS:
+  acc_lim_x: 10
+  acc_lim_theta: 10
+
+  max_vel_x: 2.23
+  max_vel_x_backwards: 2.23
+  max_vel_theta: 5.0
+
+  acc_lim_y: 0
+
+  footprint_model/type: "polygon"
+
+  #Goal Tolerance Parameters
+  xy_goal_tolerance: 0.75
+  yaw_goal_tolerance: 3.14
+
+  #Trajectory Parameters Configuration
+  min_samples: 20
+
+  #global_plan_viapoint_sep
+  odom_topic: odom
+  map_frame: map
+
 # Plans robot movement
 DWAPlannerROS: 
   # Robot Configuration Parameters
@@ -28,66 +52,71 @@ DWAPlannerROS:
   acc_lim_th: 5
 
   max_vel_trans: 2.23
-  min_vel_trans: -2.23
+  min_vel_trans: -10
   max_vel_x: 2.23
-  min_vel_x: -2.23
+  min_vel_x: -10
 
   max_vel_y: 0
   min_vel_y: 0
 
-  max_rot_vel: 2.0
-  min_rot_vel: -2.0
-  max_rot_vel: 2.0
-  min_rot_vel: -2.0
+  max_vel_theta: 5.0
+  min_vel_theta: -5.0
 
   # Goal Tolerance Parameters
   yaw_goal_tolerance: 3.14
   xy_goal_tolerance: 0.75
 
   # Forward Simulation Parameters
-  sim_time: 3.0
+  sim_time: 2.25
 
-  vx_samples: 15
+  vx_samples: 40
   vth samples: 40
 
+  #sim_granularity: 0.02
+
   # Trajectory Scoring Parameters
   path_distance_bias: 60.0
   goal_distance_bias: 20.0
-  occdist_scale: 0.02
+  occdist_scale: 0.01
 
 # Plans robot movement
-TrajectoryPlannerROS: 
-  # Robot Configuration Parameters
-  acc_lim_x: 5.0
-  acc_lim_y: 0
-  acc_lim_theta: 5.0
-
-  max_vel_x: 2.23
-  min_vel_x: -2.23
-  max_vel_theta: 0.75
-  min_vel_theta: -0.75
-  min_in_place_vel_theta: -0.75
-
-  escape_vel: -2.23 # Recovery
-
-  holonomic_robot: false # Espresso is not Holonomic
+  
+# TrajectoryPlannerROS: 
+#   # Robot Configuration Parameters
+#   acc_lim_x: 5.0
+#   acc_lim_y: 0
+#   acc_lim_theta: 5.0
+
+#   max_vel_x: 2.23
+#   min_vel_x: -2.23
+#   max_vel_theta: 0.75
+#   min_vel_theta: -0.75
+#   min_in_place_vel_theta: -0.75
+
+#   escape_vel: -2.23 # Recovery
+
+#   holonomic_robot: false # Espresso is not Holonomic
+
+#   # Goal Tolerance Parameters
+#   xy_goal_tolerance:  0.75
+#   yaw_goal_tolerance: 3.14
+
+#   # Forward Simulation Parameters
+#   sim_time: 2.0
+#   vx_samples: 15
+#   vtheta_samples: 40
+
+#   # Trajectory Scoring Parameters
+#   meter_scoring: true # Sets distances in cost function to meters instead of cells (allows different map resolutions)
+   
+#     #pdist_scale and gdist_scale changed according to DWAPlannerROS's path_distance_bias and goal_distance_bias's value used in this yaml and their default value in DWA planner. 
+#     #ex: pdist_scale used / TrajectoryPlanner default pdist_scale = DWAPlannerROS path_distance_bias used / DWAPlannerROS default path_distance_bias
+#   pdist_scale: 1.125 # Default: 0.6
+#   gdist_scale: 0.5 # Default: 0.6
+#   occdist_scale: 0.02 # Default: 0.01
+#   heading_scoring: false # Default: false
+#   heading_lookahead: 0.35 # Default: 0.35
 
-  # Goal Tolerance Parameters
-  xy_goal_tolerance:  0.75
-  yaw_goal_tolerance: 3.14
 
-  # Forward Simulation Parameters
-  sim_time: 2.0
-  vx_samples: 15
-  vtheta_samples: 40
+#### Recovery Behaviors ####
 
-  # Trajectory Scoring Parameters
-  meter_scoring: true # Sets distances in cost function to meters instead of cells (allows different map resolutions)
-   
-    #pdist_scale and gdist_scale changed according to DWAPlannerROS's path_distance_bias and goal_distance_bias's value used in this yaml and their default value in DWA planner. 
-    #ex: pdist_scale used / TrajectoryPlanner default pdist_scale = DWAPlannerROS path_distance_bias used / DWAPlannerROS default path_distance_bias
-  pdist_scale: 1.125 # Default: 0.6
-  gdist_scale: 0.5 # Default: 0.6
-  occdist_scale: 0.02 # Default: 0.01
-  heading_scoring: false # Default: false
-  heading_lookahead: 0.35 # Default: 0.35
diff --git a/nav/nav_stack/launch/move_base.launch b/nav/nav_stack/launch/move_base.launch
index 249e5a2..e87cc10 100644
--- a/nav/nav_stack/launch/move_base.launch
+++ b/nav/nav_stack/launch/move_base.launch
@@ -18,17 +18,20 @@
         
         <!-- Local Planner (Default) -->
         <!-- Use DWAPlannerROS package for local planning -->
-        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
+        <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/> -->
+
+        <!-- Use TEBlocalPlanner package for local planning -->
+        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/> 
 
         <!-- Use TrajectoryPlannerROS package for local planning by commenting out other local planners -->
 
         <!-- Global Planner -->
 
-        <!-- Use Global Planner package for global planning (Default)-->
-        <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
+        <!-- Use NavfnROS package for global planning (Default) (Performance most stable)-->
+        <param name="base_global_planner" value="navfn/NavfnROS"/>      
 
-        <!-- Use NavfnROS package for global planning (Backup) -->
-        <!-- <param name="base_global_planner" value="navfn/NavfnROS"/> -->        
+        <!-- Use Global PLanner package for global planning (still needs tuning/refining) -->  
+        <!-- <param name="base_global_planner" value="global_planner/GlobalPlanner"/> -->
     </node>
 
     <!-- TODO: always have a manual override available when doing autonomous navigation -->
diff --git a/odom/config/navsat.yaml b/odom/config/navsat.yaml
index b184e54..94239c8 100644
--- a/odom/config/navsat.yaml
+++ b/odom/config/navsat.yaml
@@ -4,7 +4,7 @@ delay: 0.0  # Simulated GPS needs no start-up time
 # world_frame: map
 
 # Initial configuration
-magnetic_declination_radians: -0.1306
+magnetic_declination_radians: -0.1302
 yaw_offset: 1.5707963 # pi/2 rad since 0 yaw == North
 zero_altitude: true
 
diff --git a/odom/config/odom_global.yaml b/odom/config/odom_global.yaml
index 349a520..668dc8e 100644
--- a/odom/config/odom_global.yaml
+++ b/odom/config/odom_global.yaml
@@ -29,7 +29,7 @@ print_diagnostics: true
 # odom1_differential: true
 # odom1_relative: false
 
-odom0: odom # Sim wheel_encoder data
+odom0: wheel_odom/euler # Real wheel_encoder data
 # odom0: fake_odom_wheel_encoder_quat # IRL wheel encoder data
 odom0_config: [false , false , false,
                false, false, false,
diff --git a/odom/config/odom_local.yaml b/odom/config/odom_local.yaml
index 1e3c82d..c810cdd 100644
--- a/odom/config/odom_local.yaml
+++ b/odom/config/odom_local.yaml
@@ -18,7 +18,7 @@ print_diagnostics: true
 
 ## Odometry Sensor(s) ##
 
-odom0: odom #Use wheel_encoder data (change once new node is up). Simulated by Gazebo
+odom0: wheel_odom/euler #Use wheel_encoder data (change once new node is up). Simulated by Gazebo
 odom0_config: [true , true , false,
                false, false, true,
                true , true , false,