From 5a41ba69abd99b17b641ebf69046ade29de13b91 Mon Sep 17 00:00:00 2001
From: Nick Ohi <mountainman@live.com>
Date: Sun, 28 Aug 2016 10:32:33 -0400
Subject: [PATCH] Change to WPI map and change search local map to uniform
 distro. Also fix potential phantom bug in avoid.

---
 lidar/src/lidar_filtering.cpp                     |  4 ++--
 robot_control/include/robot_control/map_manager.h |  8 ++++----
 robot_control/src/avoid.cpp                       |  1 +
 robot_control/src/map_manager.cpp                 | 11 ++++++-----
 4 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/lidar/src/lidar_filtering.cpp b/lidar/src/lidar_filtering.cpp
index e99c2a8..7bdc2e6 100644
--- a/lidar/src/lidar_filtering.cpp
+++ b/lidar/src/lidar_filtering.cpp
@@ -377,7 +377,7 @@ void LidarFilter::doMathMapping()
 	seg_plane.setModelType (pcl::SACMODEL_PLANE);
 	seg_plane.setMethodType (pcl::SAC_RANSAC);
 	seg_plane.setMaxIterations (1000); //max iterations for RANSAC
-	seg_plane.setDistanceThreshold (0.15); //ground detection threshold parameter
+    seg_plane.setDistanceThreshold (0.25); //ground detection threshold parameter
 	seg_plane.setInputCloud (cloud); //was raw_cloud
 
 	//segment the points fitted to the plane using ransac
@@ -1612,4 +1612,4 @@ void LidarFilter::fitCylinderLong()
 // 		stopSavingDataToFile=true; 
 // 		outputFile.close();
 // 	}
-// }
\ No newline at end of file
+// }
diff --git a/robot_control/include/robot_control/map_manager.h b/robot_control/include/robot_control/map_manager.h
index c7fdebf..0175e4f 100644
--- a/robot_control/include/robot_control/map_manager.h
+++ b/robot_control/include/robot_control/map_manager.h
@@ -26,8 +26,8 @@
 #define DEG2RAD PI/180.0
 #define RAD2DEG 180.0/PI
 
-#define EVANSDALE
-//#define WPI
+//#define EVANSDALE
+#define WPI
 //#define QUAD
 //#define CHESTNUT_RIDGE
 
@@ -175,8 +175,8 @@ class MapManager
 	int numRandomWaypointSearchDistanceCriteriaFailed;
 	const int randomWaypointDistanceCriteriaFailedLimit = 100;
 	const float mapResolution = 1.0; // m
-	const float searchLocalMapLength = 40.0; // m
-	const float searchLocalMapWidth = 40.0; // m
+	const float searchLocalMapLength = 20.0; // m
+	const float searchLocalMapWidth = 20.0; // m
 	const float sampleProbPeak = 1.0;
 	const int smoothDriveabilityNumNeighborsToChangeValue = 6;
 	const float randomWaypointMinDistance = 4.0; // m
diff --git a/robot_control/src/avoid.cpp b/robot_control/src/avoid.cpp
index 6261fe4..f470959 100644
--- a/robot_control/src/avoid.cpp
+++ b/robot_control/src/avoid.cpp
@@ -46,6 +46,7 @@ bool Avoid::runProc()
                 sendDequeClearAll();
                 ROS_INFO("AVOID gave up ROI number %i",currentROIIndex);
                 voiceSay->call("give up r o i");
+                break;
             }
             if(!execInfoMsg.actionBool3[interruptedAvoid] && (interruptedAvoid || interruptedEmergencyEscape))
             {
diff --git a/robot_control/src/map_manager.cpp b/robot_control/src/map_manager.cpp
index 9203ccd..30c980c 100644
--- a/robot_control/src/map_manager.cpp
+++ b/robot_control/src/map_manager.cpp
@@ -217,9 +217,10 @@ bool MapManager::searchMapCallback(robot_control::SearchMap::Request &req, robot
             searchLocalMapExists = true;
             //grid_map::GridMapRosConverter::fromMessage(createROIKeyframeSrv.response.keyframe.map, ROIKeyframe);
             searchLocalMapToROIAngle = RAD2DEG*atan2(regionsOfInterest.at(req.roiIndex).s, regionsOfInterest.at(req.roiIndex).e) - fmod(globalPose.heading, 360.0);
-            sigmaROIX = regionsOfInterest.at(req.roiIndex).radialAxis/2.0/numSigmasROIAxis;
-            sigmaROIY = regionsOfInterest.at(req.roiIndex).tangentialAxis/2.0/numSigmasROIAxis;
-            searchLocalMap.add(layerToString(_localMapDriveability), 0.0);
+            //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);
             for(int i=0; i<createROIHazardMapSrv.response.x_mean.size(); i++)
             {
                 searchLocalMapCoord[0] = createROIHazardMapSrv.response.x_mean.at(i);
@@ -230,12 +231,12 @@ bool MapManager::searchMapCallback(robot_control::SearchMap::Request &req, robot
                 //ROS_INFO("ROIX = %f; ROIY = %f",ROIX, ROIY);
                 if(searchLocalMap.isInside(searchLocalMapCoord)) searchLocalMap.atPosition(layerToString(_localMapDriveability), searchLocalMapCoord) = 10.0;
             }
-            for(grid_map::GridMapIterator it(searchLocalMap); !it.isPastEnd(); ++it)
+            /*for(grid_map::GridMapIterator it(searchLocalMap); !it.isPastEnd(); ++it)
             {
                 searchLocalMap.getPosition(*it, searchLocalMapCoord);
                 rotateCoord(searchLocalMapCoord[0], searchLocalMapCoord[1], ROIX, ROIY, searchLocalMapToROIAngle);
                 searchLocalMap.at(layerToString(_sampleProb), *it) = sampleProbPeak*exp(-(pow(ROIX,2.0)/(2.0*pow(sigmaROIX,2.0))+pow(ROIY,2.0)/(2.0*pow(sigmaROIY,2.0))));
-            }
+            }*/
             grid_map::GridMapRosConverter::toMessage(searchLocalMap, searchLocalMapMsg);
             searchLocalMapPub.publish(searchLocalMapMsg);
         }