Change to WPI map and change search local map to uniform distro. Also…

… fix potential phantom bug in avoid.
wvu-irl · Aug 28, 2016 · 5a41ba6 · 5a41ba6
1 parent 1d49908
commit 5a41ba6
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 11 deletions.
diff --git 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();
 // 	}
-// }
+// }
diff --git 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
@@ -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
@@ -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);
         }