diff --git a/computer_vision/data/calibration/calibration_mask.jpg b/computer_vision/data/calibration/calibration_mask.jpg
index 0c9438c..6f9ca50 100644
Binary files a/computer_vision/data/calibration/calibration_mask.jpg and b/computer_vision/data/calibration/calibration_mask.jpg differ
diff --git a/computer_vision/data/calibration/cameraCalibration.csv b/computer_vision/data/calibration/cameraCalibration.csv
index bd143f2..6d05eae 100644
--- a/computer_vision/data/calibration/cameraCalibration.csv
+++ b/computer_vision/data/calibration/cameraCalibration.csv
@@ -1,15 +1,15 @@
--0.0226893
+0.0139626
+-0.00953028
 -0.0020718
--0.0455796
 1965
 2069
 1041
-3107
+2963
 1017
 1090
 1404
 872
 120
-37
-130
-15
+58
+148
+120
diff --git a/computer_vision/scripts/ClassifierService.py b/computer_vision/scripts/ClassifierService.py
index ae7eea6..7d170c9 100755
--- a/computer_vision/scripts/ClassifierService.py
+++ b/computer_vision/scripts/ClassifierService.py
@@ -123,100 +123,100 @@ def startService(self):
 	# dictionary to store classifier parameters
 	classifierDict = {}
 
-	# classifier version
-	classifierVersion = 'CACH_V1'
-	classifierSampleType = 'CACH'
-	classifierType = 0
-
-	# imgSize
-	imgSize = 50
-	
-	# read the mean data
-	meanData50PathCach = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
-	meanData50Cach = np.load(meanData50PathCach)
-	meanData50Cach = np.reshape(meanData50Cach, (1, 3, imgSize, imgSize))
-	
-	# read the classifier 
-	classifier50PathCach = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
-
-	# set dropout parameters for better performance
-	dropoutParams50Cach = {}
-	dropoutParams50Cach['conv'] = 0.5
-	dropoutParams50Cach['fc'] = 0.5
-	
-	# initialize DeepFishNet50Cach
-	myClassifier50Cach = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Cach, mode='Test', modelToLoad = classifier50PathCach)
-	
-	# store 50 x 50 classifier details
-	classifierDict[classifierType] = {}
-	classifierDict[classifierType]['classifier'] = myClassifier50Cach
-	classifierDict[classifierType]['mean'] = meanData50Cach
-	classifierDict[classifierType]['modulePath'] = cvModulePath
-
-	# classifier version
-	classifierVersion = 'HARD_V1'
-	classifierSampleType = 'HARD'
-	classifierType = 1
-
-	# imgSize
-	imgSize = 50
-	
-	# read the mean data
-	meanData50PathHard = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
-	meanData50Hard = np.load(meanData50PathHard)
-	meanData50Hard = np.reshape(meanData50Hard, (1, 3, imgSize, imgSize))
-	
-	# read the classifier
-	classifier50PathHard = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
-
-	# set dropout parameters for better performance
-	dropoutParams50Hard = {}
-	dropoutParams50Hard['conv'] = 0.5
-	dropoutParams50Hard['fc'] = 0.5
-	
-	# initialize DeepFishNet50Hard
-	myClassifier50Hard = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Hard, mode='Test', modelToLoad = classifier50PathHard)
-	
-	# store 50 x 50 classifier details
-	classifierDict[classifierType] = {}
-	classifierDict[classifierType]['classifier'] = myClassifier50Hard
-	classifierDict[classifierType]['mean'] = meanData50Hard
-	classifierDict[classifierType]['modulePath'] = cvModulePath
-
-	# classifier version
-	classifierVersion = 'ROCK_V1'
-	classifierSampleType = 'ROCK'
-	classifierType = 2
-
-	# imgSize
-	imgSize = 50
-	
-	# read the mean data
-	meanData50PathRock = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
-	meanData50Rock = np.load(meanData50PathRock)
-	meanData50Rock = np.reshape(meanData50Rock, (1, 3, imgSize, imgSize))
-	
-	# read the classifier
-	classifier50PathRock = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
-
-	# set dropout parameters for better performance
-	dropoutParams50Rock = {}
-	dropoutParams50Rock['conv'] = 0.5
-	dropoutParams50Rock['fc'] = 0.5
-	
-	# initialize DeepFishNet50Rock
-	myClassifier50Rock = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Rock, mode='Test', modelToLoad = classifier50PathRock)
-	
-	# store 50 x 15 classifier details
-	classifierDict[classifierType] = {}
-	classifierDict[classifierType]['classifier'] = myClassifier50Rock
-	classifierDict[classifierType]['mean'] = meanData50Rock
-	classifierDict[classifierType]['modulePath'] = cvModulePath
+	# # classifier version
+	# classifierVersion = 'CACH_V1'
+	# classifierSampleType = 'CACH'
+	# classifierType = 0
+
+	# # imgSize
+	# imgSize = 50
+	
+	# # read the mean data
+	# meanData50PathCach = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
+	# meanData50Cach = np.load(meanData50PathCach)
+	# meanData50Cach = np.reshape(meanData50Cach, (1, 3, imgSize, imgSize))
+	
+	# # read the classifier 
+	# classifier50PathCach = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
+
+	# # set dropout parameters for better performance
+	# dropoutParams50Cach = {}
+	# dropoutParams50Cach['conv'] = 0.5
+	# dropoutParams50Cach['fc'] = 0.5
+	
+	# # initialize DeepFishNet50Cach
+	# myClassifier50Cach = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Cach, mode='Test', modelToLoad = classifier50PathCach)
+	
+	# # store 50 x 50 classifier details
+	# classifierDict[classifierType] = {}
+	# classifierDict[classifierType]['classifier'] = myClassifier50Cach
+	# classifierDict[classifierType]['mean'] = meanData50Cach
+	# classifierDict[classifierType]['modulePath'] = cvModulePath
+
+	# # classifier version
+	# classifierVersion = 'HARD_V1'
+	# classifierSampleType = 'HARD'
+	# classifierType = 1
+
+	# # imgSize
+	# imgSize = 50
+	
+	# # read the mean data
+	# meanData50PathHard = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
+	# meanData50Hard = np.load(meanData50PathHard)
+	# meanData50Hard = np.reshape(meanData50Hard, (1, 3, imgSize, imgSize))
+	
+	# # read the classifier
+	# classifier50PathHard = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
+
+	# # set dropout parameters for better performance
+	# dropoutParams50Hard = {}
+	# dropoutParams50Hard['conv'] = 0.5
+	# dropoutParams50Hard['fc'] = 0.5
+	
+	# # initialize DeepFishNet50Hard
+	# myClassifier50Hard = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Hard, mode='Test', modelToLoad = classifier50PathHard)
+	
+	# # store 50 x 50 classifier details
+	# classifierDict[classifierType] = {}
+	# classifierDict[classifierType]['classifier'] = myClassifier50Hard
+	# classifierDict[classifierType]['mean'] = meanData50Hard
+	# classifierDict[classifierType]['modulePath'] = cvModulePath
+
+	# # classifier version
+	# classifierVersion = 'ROCK_V1'
+	# classifierSampleType = 'ROCK'
+	# classifierType = 2
+
+	# # imgSize
+	# imgSize = 50
+	
+	# # read the mean data
+	# meanData50PathRock = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+"/allData_50_lmdb_"+classifierSampleType+".npy"
+	# meanData50Rock = np.load(meanData50PathRock)
+	# meanData50Rock = np.reshape(meanData50Rock, (1, 3, imgSize, imgSize))
+	
+	# # read the classifier
+	# classifier50PathRock = cvModulePath+'/data/classifier/DeepFishNet'+str(imgSize)+'/'+classifierVersion+'/best_9epoch_50_'+classifierSampleType+'.npz'
+
+	# # set dropout parameters for better performance
+	# dropoutParams50Rock = {}
+	# dropoutParams50Rock['conv'] = 0.5
+	# dropoutParams50Rock['fc'] = 0.5
+	
+	# # initialize DeepFishNet50Rock
+	# myClassifier50Rock = DeepFishNet50(loadData = False, imgSize = imgSize, crossvalidid = 0, dropout_params = dropoutParams50Rock, mode='Test', modelToLoad = classifier50PathRock)
+	
+	# # store 50 x 15 classifier details
+	# classifierDict[classifierType] = {}
+	# classifierDict[classifierType]['classifier'] = myClassifier50Rock
+	# classifierDict[classifierType]['mean'] = meanData50Rock
+	# classifierDict[classifierType]['modulePath'] = cvModulePath
 
 	# classifier version
 	classifierVersion = 'ALL_V1'
 	classifierSampleType = 'ALL'
-	classifierType = 3
+	classifierType = 0
 
 	# imgSize
 	imgSize = 50
diff --git a/computer_vision/src/sample_search.cpp b/computer_vision/src/sample_search.cpp
index a4571a0..6f972ae 100644
--- a/computer_vision/src/sample_search.cpp
+++ b/computer_vision/src/sample_search.cpp
@@ -421,7 +421,7 @@ bool SampleSearch::searchForSamples(messages::CVSearchCmd::Request &req, message
     ROS_INFO("Time taken in seconds for segmentation service: %f", endSegmentationTime - startSegmentationTime);
 
     //do not call classifier if no blobs or too many  blobs were extracted from segmentation
-	if(segmentImageSrv.response.coordinates.size()/2 < 1 && segmentImageSrv.response.coordinates.size()/2 > 1000)
+	if(segmentImageSrv.response.coordinates.size()/2 < 1 || segmentImageSrv.response.coordinates.size()/2 > 1000)
 	{
 		ROS_WARN("No blobs detected in image. Not performing classification.");
 		searchForSamplesMsgOut.sampleList.clear();
@@ -438,13 +438,15 @@ bool SampleSearch::searchForSamples(messages::CVSearchCmd::Request &req, message
 	gettimeofday(&this->localTimer, NULL);
     double startClassifierTime = this->localTimer.tv_sec+(this->localTimer.tv_usec/1000000.0);  
 
-    //ALL SAMPLE CLASSIFIER (THIS WILL BE REMOVED ONCE THE OTHER CLASSIFIERS ARE TRAINED)
+    //ALL SAMPLE CLASSIFIER
+	G_sample_probabilities.clear();
 	imageProbabilitiesSrv.request.numBlobs = segmentImageSrv.response.coordinates.size()/2;
-	imageProbabilitiesSrv.request.imgSize = 50; //50 will do 50x50 classifier, 150 will do 150x150 classifier (BUT 150x150 no longer exists)
-	imageProbabilitiesSrv.request.classifierType = 2;
+	imageProbabilitiesSrv.request.imgSize = 50;
+	imageProbabilitiesSrv.request.classifierType = 0;
 	if(classifierClient.call(imageProbabilitiesSrv))
 	{
-		ROS_INFO("imageProbabilitiesSrv call successful!");
+		ROS_INFO("imageProbabilitiesSrv call successful with type 0 (cach)!");
+		G_sample_probabilities = imageProbabilitiesSrv.response.responseProbabilities;
 	}
 	else
 	{
@@ -457,114 +459,6 @@ bool SampleSearch::searchForSamples(messages::CVSearchCmd::Request &req, message
 		return true;
 	}
 
-	G_cach_probabilities.clear();
-	G_hard_probabilities.clear();
-	G_rock_probabilities.clear();
-	G_sample_probabilities.clear();
-	G_sample_ids.clear();
-	if(req.roi == 0)
-	{
-		//precached sample classifier
-		imageProbabilitiesSrv.request.numBlobs = segmentImageSrv.response.coordinates.size()/2;
-		imageProbabilitiesSrv.request.imgSize = 50;
-		imageProbabilitiesSrv.request.classifierType = 3;
-		if(classifierClient.call(imageProbabilitiesSrv))
-		{
-			ROS_INFO("imageProbabilitiesSrv call successful with type 0 (cach)!");
-			G_cach_probabilities = imageProbabilitiesSrv.response.responseProbabilities;
-		}
-		else
-		{
-			ROS_ERROR("Error! Failed to call service ImageProbabilities!");
-			searchForSamplesMsgOut.sampleList.clear();
-			searchForSamplesMsgOut.procType = req.procType;
-			searchForSamplesMsgOut.serialNum = req.serialNum;
-			searchForSamplesPub.publish(searchForSamplesMsgOut);
-			ros::spinOnce();
-			return true;
-		}
-
-		//assign probabilties and sample ids
-		for(int i=0; i<G_cach_probabilities.size(); i++) //this is the number of blobs from segmentation
-		{
-			G_hard_probabilities.push_back(0);
-			G_rock_probabilities.push_back(0);
-			float tempMappedProbabilityCach = G_cach_probabilities[i];//cachSigCoef[0]+(cachSigCoef[1]-cachSigCoef[0])/(1+pow(10,( (cachSigCoef[2]-G_cach_probabilities[i])*cachSigCoef[3]) ) );
-			G_sample_probabilities.push_back(tempMappedProbabilityCach);
-			G_sample_ids.push_back(0);
-		}
-	}
-	else
-	{
-		//hard sample classifier
-		imageProbabilitiesSrv.request.numBlobs = segmentImageSrv.response.coordinates.size()/2;
-		imageProbabilitiesSrv.request.imgSize = 50;
-		imageProbabilitiesSrv.request.classifierType = 3;
-		if(classifierClient.call(imageProbabilitiesSrv))
-		{
-			ROS_INFO("imageProbabilitiesSrv call successful with type 1 (hard)!");
-			G_hard_probabilities = imageProbabilitiesSrv.response.responseProbabilities;
-		}
-		else
-		{
-			ROS_ERROR("Error! Failed to call service ImageProbabilities!");
-			searchForSamplesMsgOut.sampleList.clear();
-			searchForSamplesMsgOut.procType = req.procType;
-			searchForSamplesMsgOut.serialNum = req.serialNum;
-			searchForSamplesPub.publish(searchForSamplesMsgOut);
-			ros::spinOnce();
-			return true;
-		}
-
-		//rock sample classifier
-		imageProbabilitiesSrv.request.numBlobs = segmentImageSrv.response.coordinates.size()/2;
-		imageProbabilitiesSrv.request.imgSize = 50;
-		imageProbabilitiesSrv.request.classifierType = 3;
-		if(classifierClient.call(imageProbabilitiesSrv))
-		{
-			ROS_INFO("imageProbabilitiesSrv call successful with type 2 (rock)!");
-			G_rock_probabilities = imageProbabilitiesSrv.response.responseProbabilities;
-		}
-		else
-		{
-			ROS_ERROR("Error! Failed to call service ImageProbabilities!");
-			searchForSamplesMsgOut.sampleList.clear();
-			searchForSamplesMsgOut.procType = req.procType;
-			searchForSamplesMsgOut.serialNum = req.serialNum;
-			searchForSamplesPub.publish(searchForSamplesMsgOut);
-			ros::spinOnce();
-			return true;
-		}
-
-		//make sure the number of blobs is correct
-		if(G_rock_probabilities.size() != G_hard_probabilities.size() || G_rock_probabilities.size() != segmentImageSrv.response.coordinates.size()/2)
-		{
-			ROS_ERROR("Error! The size of G_rock_probabilities and G_hard_probabilities should be equal to the number of blobs from segmentation!");
-			G_hard_probabilities.clear();
-			G_rock_probabilities.clear();
-		}
-
-		//assign probabilties and sample ids (use higher probability and the cooresponding id)
-		G_sample_probabilities.clear();
-		G_sample_ids.clear();
-		for(int i=0; i<G_rock_probabilities.size(); i++) //this is the number of blobs in rock and hard probabilities
-		{
-			G_cach_probabilities.push_back(0);
-			float tempMappedProbabilityHard = G_hard_probabilities[i];//hardSigCoef[0]+(hardSigCoef[1]-hardSigCoef[0])/(1+pow(10,( (hardSigCoef[2]-G_hard_probabilities[i])*hardSigCoef[3]) ) );
-			float tempMappedProbabilityRock = G_rock_probabilities[i];//rockSigCoef[0]+(rockSigCoef[1]-rockSigCoef[0])/(1+pow(10,( (rockSigCoef[2]-G_rock_probabilities[i])*rockSigCoef[3]) ) );
-			if(tempMappedProbabilityHard >= tempMappedProbabilityRock)
-			{
-				G_sample_probabilities.push_back(tempMappedProbabilityHard);
-				G_sample_ids.push_back(1);
-			}
-			else
-			{
-				G_sample_probabilities.push_back(tempMappedProbabilityRock);
-				G_sample_ids.push_back(0);
-			}
-		}
-	}
-
 	gettimeofday(&this->localTimer, NULL);  
     double endClassifierTime = this->localTimer.tv_sec+(this->localTimer.tv_usec/1000000.0);  
     ROS_INFO("Time taken in seconds for classifier service: %f", endClassifierTime - startClassifierTime);
@@ -610,7 +504,7 @@ bool SampleSearch::searchForSamples(messages::CVSearchCmd::Request &req, message
 	gettimeofday(&this->localTimer, NULL);
     double startSaveBlobsTime = this->localTimer.tv_sec+(this->localTimer.tv_usec/1000000.0);  
 
-	saveLowAndHighProbabilityBlobs(G_sample_probabilities, segmentImageSrv.response.coordinates);
+	//saveLowAndHighProbabilityBlobs(G_sample_probabilities, segmentImageSrv.response.coordinates);
 
 	gettimeofday(&this->localTimer, NULL);  
     double endSaveBlobsTime = this->localTimer.tv_sec+(this->localTimer.tv_usec/1000000.0);  
@@ -666,38 +560,51 @@ bool SampleSearch::searchForSamples(messages::CVSearchCmd::Request &req, message
 		}
 
 		//confidence of sample
-		sampleProps.confidenceCach = G_cach_probabilities[blobsOfInterest[i]];
-		sampleProps.confidenceHard = G_hard_probabilities[blobsOfInterest[i]];
-		sampleProps.confidenceRock = G_rock_probabilities[blobsOfInterest[i]];
+		sampleProps.confidenceCach = G_sample_probabilities[blobsOfInterest[i]];
+		sampleProps.confidenceHard = G_sample_probabilities[blobsOfInterest[i]];
+		sampleProps.confidenceRock = G_sample_probabilities[blobsOfInterest[i]];
 		sampleProps.confidence = G_sample_probabilities[blobsOfInterest[i]];
 
 		//only publish results depending on expected probabilities
 		bool publish_sample_info = false;
+		bool lower_request_threshold = true; //lower the threshold unless 1 sample > 0.6
 		if(req.white > 0.6)
 		{
 			if(sampleProps.white == true) publish_sample_info = true;
+			lower_request_threshold = false;
 		}
-		else if(req.silver > 0.6)
+		
+		if(req.silver > 0.6)
 		{
 			if(sampleProps.silver == true) publish_sample_info = true;
+			lower_request_threshold = false;
 		}
-		else if(req.blueOrPurple > 0.6)
+		
+		if(req.blueOrPurple > 0.6)
 		{
 			if(sampleProps.blueOrPurple == true) publish_sample_info = true;
+			lower_request_threshold = false;
 		}
-		else if(req.pink > 0.6 || req.red > 0.6)
+		
+		if(req.pink > 0.6 || req.red > 0.6)
 		{
 			if(sampleProps.red == true || sampleProps.pink == true) publish_sample_info = true;
+			lower_request_threshold = false;
 		}
-		else if(req.orange > 0.6)
+		
+		if(req.orange > 0.6)
 		{
 			if(sampleProps.orange == true) publish_sample_info = true;
+			lower_request_threshold = false;
 		}
-		else if(req.yellow > 0.6)
+		
+		if(req.yellow > 0.6)
 		{
 			if(sampleProps.yellow == true) publish_sample_info = true;	
+			lower_request_threshold = false;
 		}
-		else
+		
+		if(lower_request_threshold == true)
 		{
 			if(req.white > 0.03) if(sampleProps.white == true) publish_sample_info = true;
 			if(req.silver > 0.03) if(sampleProps.silver == true) publish_sample_info = true;
diff --git a/lidar/CMakeLists.txt b/lidar/CMakeLists.txt
index c21d994..230c3c1 100644
--- a/lidar/CMakeLists.txt
+++ b/lidar/CMakeLists.txt
@@ -125,6 +125,7 @@ add_definitions("-std=gnu++11")
 # include_directories(include)
 include_directories(
   include
+  ~/cataglyphis_ws/src/robot_control/include
   ${PCL_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
 )
diff --git a/lidar/include/lidar/collision_detection.hpp b/lidar/include/lidar/collision_detection.hpp
index f583528..e795e1c 100644
--- a/lidar/include/lidar/collision_detection.hpp
+++ b/lidar/include/lidar/collision_detection.hpp
@@ -1,5 +1,6 @@
 #ifndef COLLISION_DETECTION_H
 #define COLLISION_DETECTION_H
+#include <robot_control/mission_planning_types_defines.h>
 #include "ros/ros.h"
 #include "ros/console.h"
 #include <iostream>
@@ -45,6 +46,7 @@
 #include <messages/CreateROIHazardMap.h>
 #include <messages/NextWaypointOut.h>
 #include <messages/RobotPose.h>
+#include <messages/MissionPlanningInfo.h>
 // #include <messages/ZedCollisionOut.h>
 #include "pcl/conversions.h"
 #include "sensor_msgs/PointCloud2.h"
@@ -62,6 +64,7 @@ class CollisionDetection
 	ros::Subscriber _sub_waypoint;
 	ros::Subscriber _sub_position;
 	ros::Subscriber _sub_zedcollision;
+	ros::Subscriber _sub_mission;
 	// ros::ServerService ;
 	void setPreviousCounters();
 	bool newPointCloudAvailable();
@@ -89,7 +92,7 @@ class CollisionDetection
 	const float _SAFE_ENVELOPE_ANGLE = 15.0*3.14159265/180.0; //safe envelope angle (radians)
 	const short int _TRIGGER_POINT_THRESHOLD = 10; //number of points in safe envelope that will trigger the avoidance
 
-	const double PI = 3.141592653589793238462643383279502884197169399375105820974944592307816406286;
+	// const double PI = 3.141592653589793238462643383279502884197169399375105820974944592307816406286;
 
 	//parameters
 	double short_distance;
@@ -102,6 +105,7 @@ class CollisionDetection
 	int threshold_counter_lidar;
 	// int threshold_counter_zed;
 	int threshold_counter_ransac;
+	int threshold_counter_ransac_avoid;
 
 	double error_angle;
 
@@ -117,11 +121,21 @@ class CollisionDetection
 	//zed data
 	// int _zedcollision;
 
+	//mission planning info
+	bool _doingApproach;
+	bool _doingExamine;
+	int _currentROI;
+
 	//counters
 	int _collision_counter_lidar_slowdown;
 	int _collision_counter_lidar_avoid;
 	// int _collision_counter_zed;
 	int _collision_counter_ransac;
+	int _collision_counter_ransac_slowdown;
+	int _collision_counter_ransac_avoid;
+
+	bool _collision_counter_ransac_switch;
+
 
 	std::vector<float> _hazard_x;
 	std::vector<float> _hazard_y;
@@ -143,7 +157,7 @@ class CollisionDetection
 	void waypointsCallback(messages::NextWaypointOut const &waypoint_msg);
 	void positionCallback(messages::RobotPose const &position_msg);
 	// void zedcollisionCallback(messages::ZedCollisionOut const &zedcollisionout_msg);
-
+	void missionCallback(messages::MissionPlanningInfo const &msg);
 	int firstChoice(double angle, double distance);
 	int secondChoice(double angle, double distance, double xg, double yg);
 	int finalChoice(double left_angle, double right_angle, int collision_left_counter, int collision_right_counter, double xg_local, double yg_local);
diff --git a/lidar/src/collision_detection.cpp b/lidar/src/collision_detection.cpp
index caeddde..0aa29c5 100644
--- a/lidar/src/collision_detection.cpp
+++ b/lidar/src/collision_detection.cpp
@@ -21,12 +21,18 @@ CollisionDetection::CollisionDetection()
 	_sub_waypoint = _nh.subscribe("/control/exec/nextwaypoint", 1, &CollisionDetection::waypointsCallback, this);
 	_sub_position = _nh.subscribe("/hsm/masterexec/globalpose", 1, &CollisionDetection::positionCallback, this);
 	// _sub_zedcollision = _nh.subscribe("/zedcollisiondetectionout", 1, &CollisionDetection::zedcollisionCallback, this);
+	_sub_mission = _nh.subscribe("/control/missionplanning/info", 1, &CollisionDetection::missionCallback, this);
 
 	//predictive avoidance service
 	returnHazardMapServ = _nh.advertiseService("/lidar/collisiondetection/createroihazardmap", &CollisionDetection::returnHazardMap, this);
 	
 	//collision output
 	_collision_status = 0;
+
+	//mission planning variables
+	_doingApproach = false;
+	_doingExamine = false;
+	_currentROI = 0;
 }
 
 void CollisionDetection::Initializations()
@@ -42,6 +48,9 @@ void CollisionDetection::Initializations()
 	threshold_counter_lidar = 2;	//counter should be bigger than threshold, than do something
 	// threshold_counter_zed = 5; 
 	threshold_counter_ransac = 2;
+	threshold_counter_ransac_avoid = 1;
+
+	_collision_counter_ransac_switch = true; //use ransac
 
 	//zed data
 	// _zedcollision = 0;
@@ -51,6 +60,8 @@ void CollisionDetection::Initializations()
 	_collision_counter_lidar_avoid = 0;
 	// _collision_counter_zed = 0;
 	_collision_counter_ransac = 0;
+	_collision_counter_ransac_slowdown = 0;
+	_collision_counter_ransac_avoid = 0;
 
 	_slowdown = false;
 	_distance_to_drive = 0;
@@ -107,6 +118,26 @@ void CollisionDetection::registrationCallback(pcl::PointCloud<pcl::PointXYZI> co
     pcl::transformPointCloud(temp_cloud, _input_cloud, T_temporary);
 }
 
+void CollisionDetection::missionCallback(messages::MissionPlanningInfo const &msg)
+{
+	if(msg.procsBeingExecuted.at(__approach__)==true) _doingApproach = true;
+	else _doingApproach = false;
+	
+	if(msg.procsBeingExecuted.at(__examine__)==true) _doingExamine = true;
+	else _doingExamine = false;
+
+	_currentROI = msg.currentROIIndex;
+
+	if((_doingApproach == true || _doingExamine == true) && _currentROI !=7)
+	{
+		_collision_counter_ransac_switch = false;
+	}
+	else
+	{
+		_collision_counter_ransac_switch = true;
+	}
+}
+
 void CollisionDetection::setPreviousCounters()
 {
 	_registration_counter_prev = _registration_counter;
@@ -223,7 +254,7 @@ int CollisionDetection::doMathSafeEnvelope() // FIRST LAYER: SAFE ENVELOPE
 	int ransacCollisionStatus = 0;
 	// if(_collision_counter_lidar_avoid == 0)
 	//if the robot do slow down, that means it detects an obstacle, otherwise, we need use RANSAC to confirm
-	if(_collision_counter_lidar_slowdown < threshold_counter_lidar)	
+	if(_collision_counter_lidar_avoid < threshold_counter_lidar && _collision_counter_ransac_switch)	
 	{
 		ransacCollisionStatus = doMathRANSAC();
 	}
@@ -237,6 +268,15 @@ int CollisionDetection::doMathSafeEnvelope() // FIRST LAYER: SAFE ENVELOPE
 		_collision_counter_ransac=0;
 	}
 	
+	if(_collision_counter_ransac >= threshold_counter_ransac && _collision_counter_ransac_slowdown < 2)
+	{
+		_collision_counter_ransac_slowdown = 2;
+	}
+
+	if(_collision_counter_ransac_slowdown >= 8)
+	{
+		_collision_counter_ransac_avoid = 1;
+	}
 
 	// ROS_INFO_STREAM("collision_point_counter: " << collision_point_counter);
 	//check zed input
@@ -264,7 +304,7 @@ int CollisionDetection::doMathSafeEnvelope() // FIRST LAYER: SAFE ENVELOPE
 	// if(collision_point_counter > _TRIGGER_POINT_THRESHOLD || _zedcollision != 0)
 	// if(collision_point_counter > _TRIGGER_POINT_THRESHOLD)
 	// if(_collision_counter_lidar_avoid >= threshold_counter_lidar || _collision_counter_zed >= threshold_counter_zed || _collision_counter_ransac >= threshold_counter_ransac)
-	if(_collision_counter_lidar_avoid >= threshold_counter_lidar || _collision_counter_ransac >= threshold_counter_ransac)
+	if(_collision_counter_lidar_avoid >= threshold_counter_lidar || _collision_counter_ransac_avoid >= threshold_counter_ransac_avoid)
 	{	
 		_collision_status = 1;	//detected a obstacle
 
@@ -404,9 +444,12 @@ int CollisionDetection::doMathSafeEnvelope() // FIRST LAYER: SAFE ENVELOPE
 		_collision_counter_lidar_slowdown = 0;
 		// _collision_counter_zed = 0;
 		_collision_counter_ransac = 0;
+		_collision_counter_ransac_slowdown = 0;
+		_collision_counter_ransac_avoid = 0;
 	}
-	else if(_collision_counter_lidar_slowdown >= threshold_counter_lidar)
+	else if(_collision_counter_lidar_slowdown >= threshold_counter_lidar || _collision_counter_ransac_slowdown > 1)
 	{
+		_collision_counter_ransac_slowdown = _collision_counter_ransac_slowdown * _collision_counter_ransac_slowdown;
 		_collision_status = 0;
 		_slowdown = true;
 		_distance_to_drive = 0;
@@ -455,7 +498,7 @@ int CollisionDetection::doMathRANSAC() // FIRST LAYER: SAFE ENVELOPE
 		seg_plane.setModelType (pcl::SACMODEL_PLANE);
 		seg_plane.setMethodType (pcl::SAC_RANSAC);
 		seg_plane.setMaxIterations (1000); //max iterations for RANSAC
-		seg_plane.setDistanceThreshold (0.25); //ground detection threshold parameter
+		seg_plane.setDistanceThreshold (0.15); //ground detection threshold parameter
 		seg_plane.setInputCloud (cloud); //was raw_cloud
 		// std::cout << "cloud->points.size() = " << cloud->points.size() << std::endl;
 
diff --git a/messages/msg/MissionPlanningInfo.msg b/messages/msg/MissionPlanningInfo.msg
index 48409da..b614b37 100644
--- a/messages/msg/MissionPlanningInfo.msg
+++ b/messages/msg/MissionPlanningInfo.msg
@@ -40,3 +40,4 @@ bool[] procsToResume
 int32[] procStates
 uint8 collisionCondition
 float64 missionTime
+uint32 currentROIIndex
diff --git a/messages/srv/MissionPlanningControl.srv b/messages/srv/MissionPlanningControl.srv
index 3903634..8d8a4b1 100644
--- a/messages/srv/MissionPlanningControl.srv
+++ b/messages/srv/MissionPlanningControl.srv
@@ -40,4 +40,5 @@ bool[] procsToResume
 int32[] procStates
 uint8 collisionCondition
 float64 missionTime
+uint32 currentROIIndex
 ---
diff --git a/navigation/src/navigation_filter.cpp b/navigation/src/navigation_filter.cpp
index f563d93..f7e5ca3 100644
--- a/navigation/src/navigation_filter.cpp
+++ b/navigation/src/navigation_filter.cpp
@@ -22,11 +22,11 @@ NavigationFilter::NavigationFilter()
 	registration_counter = 0;
 	registration_counter_prev = 0;
 
-	filter.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
-	init_filter.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
-	filter1.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
-	filter2.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
-	filterS.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+	filter.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+	init_filter.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+	filter1.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+	filter2.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+	filterS.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
 
 	encoders.set_wheel_radius(0.2286/2);
 	encoders.set_counts_per_revolution(4476.16*1.062);
@@ -249,34 +249,36 @@ void NavigationFilter::waiting(User_Input_Nav_Act user_input_nav_act)
 void NavigationFilter::forklift_drive(User_Input_Nav_Act user_input_nav_act)
 {
     init_filter.which_nb_to_keep(imu.nb1_drive_counter, imu.nb1_current, imu.nb1_good, imu.nb1_good_prev, imu.nb2_drive_counter, imu.nb2_current, imu.nb2_good, imu.nb2_good_prev, imu.nbS_current, imu.nbS_good, imu.nbS_good_prev);
-	if (imu.new_nb1!=0)
+	if ((fabs(sqrt(imu.ax*imu.ax+imu.ay*imu.ay+imu.az*imu.az)-1)> 0.05 || sqrt((imu.p)*(imu.p)+(imu.q)*(imu.q)+(imu.r)*(imu.r))>0.005) || encoders.delta_distance != 0)
 	{
-		filter1.turning(imu.nb1_p,imu.nb1_q,imu.nb1_r,imu.dt1);
-	}
-	else
-	{
-		filter1.blind_turning(imu.nb1_p,imu.nb1_q,imu.nb1_r,imu.dt1);
-	}
+		if (imu.new_nb1!=0)
+		{
+			filter1.turning(imu.nb1_p,imu.nb1_q,imu.nb1_r,imu.dt1);
+		}
+		else
+		{
+			filter1.blind_turning(imu.nb1_p,imu.nb1_q,imu.nb1_r,imu.dt1);
+		}
 
-	if (imu.new_nb2!=0)
-	{
-		filter2.turning(imu.nb2_p,imu.nb2_q,imu.nb2_r,imu.dt2);
-	}
-	else
-	{
-		filter2.blind_turning(imu.nb2_p,imu.nb2_q,imu.nb2_r,imu.dt2);
-	}
+		if (imu.new_nb2!=0)
+		{
+			filter2.turning(imu.nb2_p,imu.nb2_q,imu.nb2_r,imu.dt2);
+		}
+		else
+		{
+			filter2.blind_turning(imu.nb2_p,imu.nb2_q,imu.nb2_r,imu.dt2);
+		}
 
-	if (imu.new_nbS!=0)
-	{
-		filterS.turning(imu.nbS_p,imu.nbS_q,imu.nbS_r,imu.dtS);
-	}
-	else
-	{
-		filterS.blind_turning(imu.nbS_p,imu.nbS_q,imu.nbS_r,imu.dtS);
+		if (imu.new_nbS!=0)
+		{
+			filterS.turning(imu.nbS_p,imu.nbS_q,imu.nbS_r,imu.dtS);
+		}
+		else
+		{
+			filterS.blind_turning(imu.nbS_p,imu.nbS_q,imu.nbS_r,imu.dtS);
+		}
 	}
 
-
 	if(init_filter.keep_nb == 3 || init_filter.keep_nb == 28 || init_filter.keep_nb == 1 || init_filter.keep_nb == 5 || init_filter.keep_nb == 7 || init_filter.keep_nb == 9 || init_filter.keep_nb == 11 || init_filter.keep_nb == 12 || init_filter.keep_nb == 14 || init_filter.keep_nb == 20 || init_filter.keep_nb == 22 || init_filter.keep_nb == 24 || init_filter.keep_nb == 26 || init_filter.keep_nb == 29 || init_filter.keep_nb == 31 || init_filter.keep_nb == 33 || init_filter.keep_nb == 35)
 	{
 		init_filter.initialize_states(filter1.phi,filter1.theta,filter1.psi,filter1.x,filter1.y,filter1.P_phi,filter1.P_theta,filter1.P_psi,filter1.P_x,filter1.P_y); //phi,theta,psi,x,y,P_phi,P_theta,P_psi,P_x,P_y
@@ -461,10 +463,10 @@ void NavigationFilter::forklift_drive(User_Input_Nav_Act user_input_nav_act)
 		filter.P_north_angle = init_filter.P_psi;
 
 		state = _run; 
-		filter.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
-		filter1.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
-		filter2.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
-		filterS.initialize_states(0,0,0,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
+		filter.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
+		filter1.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
+		filter2.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y);
+		filterS.initialize_states(0,0,PI,1,0,filter.P_phi,filter.P_theta,filter.P_psi,filter.P_x,filter.P_y); 
 	}
 	else state = _forklift_drive;
 }
diff --git a/robot_control/include/robot_control/initialize.h b/robot_control/include/robot_control/initialize.h
index f0d18ff..bb33b00 100644
--- a/robot_control/include/robot_control/initialize.h
+++ b/robot_control/include/robot_control/initialize.h
@@ -9,7 +9,7 @@ class Initialize : public Procedure
 public:
 	INITIALIZE_STEP_T step;
 	bool runProc();
-	const float driveOffPlatformDistance = 4.0; // m
+	const float driveOffPlatformDistance = -4.0; // m
 	const float initWaitTime = 2.0; // sec
 };
 
diff --git a/robot_control/include/robot_control/procedure.h b/robot_control/include/robot_control/procedure.h
index ad342ad..4272d7e 100644
--- a/robot_control/include/robot_control/procedure.h
+++ b/robot_control/include/robot_control/procedure.h
@@ -33,7 +33,7 @@ class Procedure : public MissionPlanningProcedureShare
 	void sendPause();
 	void sendUnPause();
 	void findHighestConfSample();
-	void computeSampleValuesWithExpectedDistance();
+	void computeSampleValuesWithExpectedDistance(bool useHistoryValue);
 	void computeExpectedSampleLocation();
 	void clearSampleHistory();
 	void startSampleHistory(float& conf, bool* sampleTypes);
@@ -650,9 +650,12 @@ void Procedure::findHighestConfSample()
 	}
 }
 
-void Procedure::computeSampleValuesWithExpectedDistance()
+void Procedure::computeSampleValuesWithExpectedDistance(bool useHistoryValue)
 {
-	sampleDistanceAdjustedConf = sampleConfidenceGain*sampleHistoryModifiedConf -
+    float sampleConf;
+    if(useHistoryValue) sampleConf = sampleHistoryModifiedConf;
+    else sampleConf = highestConfSample.confidence;
+	sampleDistanceAdjustedConf = sampleConfidenceGain*sampleConf -
 							(sampleDistanceToExpectedGain*sqrt(pow(highestConfSample.distance,2.0)+pow(expectedSampleDistance,2.0)-
 								2.0*highestConfSample.distance*expectedSampleDistance*
 									cos(DEG2RAD*(highestConfSample.bearing-expectedSampleAngle))));
diff --git a/robot_control/include/robot_control/wpi_rois.h b/robot_control/include/robot_control/wpi_rois.h
index 64c75ca..17c0efc 100644
--- a/robot_control/include/robot_control/wpi_rois.h
+++ b/robot_control/include/robot_control/wpi_rois.h
@@ -13,11 +13,11 @@ ROI.hardLockout = false;
 ROI.roiGroup = 0;
 ROI.whiteProb = 1.0;
 ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.blueOrPurpleProb = 0.0;
+ROI.pinkProb = 0.0;
+ROI.redProb = 0.0;
+ROI.orangeProb = 0.0;
+ROI.yellowProb = 0.0;
 regionsOfInterest.push_back(ROI);
 ROI.e = 17.8 - satMapStartE; // 1
 ROI.s = 23.5 - satMapStartS;
@@ -29,13 +29,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 1;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 17.0 - satMapStartE; // 2
 ROI.s = 47.2 - satMapStartS;
@@ -47,13 +47,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 1;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 39.2 - satMapStartE; // 3
 ROI.s = 16.2 - satMapStartS;
@@ -65,13 +65,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 1;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 74.5 - satMapStartE; // 4
 ROI.s = 17.2 - satMapStartS;
@@ -83,13 +83,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 1;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 92.5 - satMapStartE; // 5
 ROI.s = 58.2 - satMapStartS;
@@ -101,13 +101,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 94.2 - satMapStartE; // 6
 ROI.s = 97.0 - satMapStartS;
@@ -119,13 +119,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 1;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 140.0 - satMapStartE; // 7
 ROI.s = 47.2 - satMapStartS;
@@ -137,13 +137,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 1;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 163.0 - satMapStartE; // 8
 ROI.s = 81.0 - satMapStartS;
@@ -155,13 +155,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 151.0 - satMapStartE; // 9
 ROI.s = 109.5 - satMapStartS;
@@ -173,13 +173,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 188.8 - satMapStartE; // 10
 ROI.s = 116.0 - satMapStartS;
@@ -191,13 +191,13 @@ ROI.allocatedTime = 270.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 194.0 - satMapStartE; // 11
 ROI.s = 145.2 - satMapStartS;
@@ -209,13 +209,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 1;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 228.8 - satMapStartE; // 12
 ROI.s = 109.2 - satMapStartS;
@@ -227,13 +227,13 @@ ROI.allocatedTime = 270.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 0;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 257.5 - satMapStartE; // 13
 ROI.s = 76.5 - satMapStartS;
@@ -245,13 +245,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 2;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 262.5 - satMapStartE; // 14
 ROI.s = 51.0 - satMapStartS;
@@ -263,13 +263,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 1;
 ROI.hardLockout = false;
 ROI.roiGroup = 2;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 ROI.e = 287.2 - satMapStartE; // 15
 ROI.s = 44.2 - satMapStartS;
@@ -281,13 +281,13 @@ ROI.allocatedTime = 180.0;
 ROI.highRisk = 0;
 ROI.hardLockout = false;
 ROI.roiGroup = 2;
-ROI.whiteProb = 1.0;
-ROI.silverProb = 1.0;
-ROI.blueOrPurpleProb = 1.0;
-ROI.pinkProb = 1.0;
-ROI.redProb = 1.0;
-ROI.orangeProb = 1.0;
-ROI.yellowProb = 1.0;
+ROI.whiteProb = 0.2;
+ROI.silverProb = 0.2;
+ROI.blueOrPurpleProb = 0.2;
+ROI.pinkProb = 0.07;
+ROI.redProb = 0.07;
+ROI.orangeProb = 0.07;
+ROI.yellowProb = 0.07;
 regionsOfInterest.push_back(ROI);
 
 #endif // WPI_ROIS_H
diff --git a/robot_control/src/approach.cpp b/robot_control/src/approach.cpp
index 5bbc0f2..8372db1 100644
--- a/robot_control/src/approach.cpp
+++ b/robot_control/src/approach.cpp
@@ -15,7 +15,7 @@ bool Approach::runProc()
 	switch(state)
 	{
 	case _init_:
-		avoidLockout = true;
+		avoidLockout = false;
 		procsBeingExecuted[procType] = true;
 		procsToExecute[procType] = false;
         procsToResume[procType] = false;
@@ -32,7 +32,7 @@ bool Approach::runProc()
 		state = _exec_;
 		break;
 	case _exec_:
-		avoidLockout = true;
+		avoidLockout = false;
 		procsBeingExecuted[procType] = true;
 		procsToExecute[procType] = false;
         procsToResume[procType] = false;
@@ -41,7 +41,7 @@ bool Approach::runProc()
 		{
 		case _computeManeuver:
 			sampleTypeMux = 0;
-			computeSampleValuesWithExpectedDistance();
+			computeSampleValuesWithExpectedDistance(true);
             if(sampleDistanceAdjustedConf >= approachValueThreshold && definiteSample) approachableSample = true;
 			else approachableSample = false;
 			if(approachableSample)
diff --git a/robot_control/src/confirm_collect.cpp b/robot_control/src/confirm_collect.cpp
index f09d920..ac906d0 100644
--- a/robot_control/src/confirm_collect.cpp
+++ b/robot_control/src/confirm_collect.cpp
@@ -33,9 +33,9 @@ bool ConfirmCollect::runProc()
         procsToResume[procType] = false;
         if(searchEnded())
 		{
-			computeSampleValuesWithExpectedDistance();
+			computeSampleValuesWithExpectedDistance(false);
             ROS_INFO("confirmCollect sampleDistanceAdjustedConf = %f",sampleDistanceAdjustedConf);
-            if(sampleDistanceAdjustedConf < confirmCollectValueThreshold) noSampleOnGround = true;
+            if((sampleDistanceAdjustedConf < confirmCollectValueThreshold) && sampleHistoryTypeMatch(highestConfSample.types)) noSampleOnGround = true;
 			else noSampleOnGround = false;
 			if(noSampleOnGround)
 			{
diff --git a/robot_control/src/grabber_set_drop.cpp b/robot_control/src/grabber_set_drop.cpp
index ab908d2..7024ac0 100644
--- a/robot_control/src/grabber_set_drop.cpp
+++ b/robot_control/src/grabber_set_drop.cpp
@@ -23,7 +23,8 @@ int GrabberSetDrop::run()
         else {returnValue_ = 0; state_ = _normal_;}
         break;
     case _recovering_:
-        robotOutputs.dropPosCmd = failsafePosition_;
+        if(dropPos_ == GRABBER_DROPPED) robotOutputs.dropPosCmd = GRABBER_RAISED;
+        else robotOutputs.dropPosCmd = GRABBER_DROPPED;
         robotOutputs.grabberStopCmd = 0;
         if(dropEnded_) {returnValue_ = 0; numFailedAttempts_++; state_ = _normal_;}
         else {returnValue_ = 0; state_ = _recovering_;}
diff --git a/robot_control/src/grabber_set_slides.cpp b/robot_control/src/grabber_set_slides.cpp
index 54f219c..f58e8d9 100644
--- a/robot_control/src/grabber_set_slides.cpp
+++ b/robot_control/src/grabber_set_slides.cpp
@@ -23,7 +23,8 @@ int GrabberSetSlides::run()
         else {returnValue_ = 0; state_ = _normal_;}
         break;
     case _recovering_:
-        robotOutputs.slidePosCmd = failsafePosition_;
+        if(slidePos_== GRABBER_CLOSED) robotOutputs.slidePosCmd = GRABBER_OPEN;
+        else robotOutputs.slidePosCmd = GRABBER_CLOSED;
         robotOutputs.grabberStopCmd = 0;
         if(slidesEnded_) {returnValue_ = 0; numFailedAttempts_++; state_ = _normal_;}
         else {returnValue_ = 0; state_ = _recovering_;}
diff --git a/robot_control/src/map_manager.cpp b/robot_control/src/map_manager.cpp
index 30c980c..174db2d 100644
--- a/robot_control/src/map_manager.cpp
+++ b/robot_control/src/map_manager.cpp
@@ -220,7 +220,8 @@ bool MapManager::searchMapCallback(robot_control::SearchMap::Request &req, robot
             //sigmaROIX = regionsOfInterest.at(req.roiIndex).radialAxis/2.0/numSigmasROIAxis;
             //sigmaROIY = regionsOfInterest.at(req.roiIndex).tangentialAxis/2.0/numSigmasROIAxis;
             searchLocalMap.setGeometry(grid_map::Length(regionsOfInterest.at(searchLocalMapROINum).radialAxis*2.0, regionsOfInterest.at(searchLocalMapROINum).tangentialAxis*2.0), mapResolution, grid_map::Position(0.0, 0.0));
-            searchLocalMap.add(layerToString(_localMapDriveability), 1.0);
+            searchLocalMap.add(layerToString(_localMapDriveability), 0.0);
+            searchLocalMap.add(layerToString(_sampleProb), 1.0);
             for(int i=0; i<createROIHazardMapSrv.response.x_mean.size(); i++)
             {
                 searchLocalMapCoord[0] = createROIHazardMapSrv.response.x_mean.at(i);
diff --git a/robot_control/src/mission_planning.cpp b/robot_control/src/mission_planning.cpp
index 769ca96..bf7b409 100644
--- a/robot_control/src/mission_planning.cpp
+++ b/robot_control/src/mission_planning.cpp
@@ -239,6 +239,13 @@ void MissionPlanning::evalConditions_()
                         shouldExecuteAvoidManeuver = false;
                     }
                 }
+                else if(procsToInterrupt[__examine__] || procsToInterrupt[__approach__])
+                {
+                    avoid.sendDequeClearAll();
+                    shouldExecuteAvoidManeuver = false;
+                    possibleSample = false;
+                    definiteSample = false;
+                }
                 /*robotStatus.pauseSwitch = true;
                 pause.sendPause();
                 std::cout << "press enter to continue" << std::endl;
@@ -550,6 +557,7 @@ void MissionPlanning::packAndPubInfoMsg_()
     }
     infoMsg.collisionCondition = collisionMsg.collision;
     infoMsg.missionTime = missionTime;
+    infoMsg.currentROIIndex = currentROIIndex;
     infoPub.publish(infoMsg);
 }
 
@@ -761,6 +769,7 @@ bool MissionPlanning::controlCallback_(messages::MissionPlanningControl::Request
     }
     collisionMsg.collision = req.collisionCondition;
     missionTime = req.missionTime;
+    currentROIIndex = req.currentROIIndex;
     return true;
 }