From 97005730d88483aaa53842d81ffb7017473da4b0 Mon Sep 17 00:00:00 2001
From: Rescue Group <rescue@rescuelap3.(none)>
Date: Thu, 25 Jul 2013 17:04:44 +1000
Subject: [PATCH] MM: Wall search extended to find expansion if wall/restricted
 not available.

---
 crosbot_explore/src/explorer.cpp | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp
index 823b4ab..36e72f1 100644
--- a/crosbot_explore/src/explorer.cpp
+++ b/crosbot_explore/src/explorer.cpp
@@ -175,6 +175,33 @@ Point Explorer::findWall(const VoronoiGrid& voronoi, const Pose& robot, double s
 		}
 	}
 
+	for (double rayAngle = 0; rayAngle < 2*M_PIl && !CELL_IS_VALID(wallCell, voronoi); rayAngle += search.angleStep) {
+		if (search.side == SearchParameters::Left) {
+			cosRay = cos(Y + startAngle - rayAngle);
+			sinRay = sin(Y + startAngle - rayAngle);
+		} else {
+			cosRay = cos(Y + startAngle + rayAngle);
+			sinRay = sin(Y + startAngle + rayAngle);
+		}
+
+		for (double ray = 0; ray <= (search.searchDistance / voronoi.resolution) &&
+								!CELL_IS_VALID(wallCell, voronoi); ray += search.rayStep) {
+			cell = Index2D(robotCell.x + ray * cosRay, robotCell.y + ray * sinRay);
+
+			if (!CELL_IS_VALID(cell, voronoi))
+				continue;
+
+			const VoronoiGrid::VoronoiCell& vCell = voronoi.getVoronoiCell(cell);
+			if (vCell.status & VoronoiGrid::VoronoiCell::Wall) {
+				wallCell = cell;
+			} else if ((vCell.status & VoronoiGrid::VoronoiCell::Restricted) ||
+					(vCell.status & VoronoiGrid::VoronoiCell::PatiallyRestricted) ||
+					(vCell.status & VoronoiGrid::VoronoiCell::Expansion)) {
+				wallCell = vCell.nearestWallCell;
+			}
+		}
+	}
+
 	if (CELL_IS_VALID(wallCell, voronoi)) {
 		return voronoi.getPosition(wallCell);
 	}
-- 
GitLab