I'm working on a grad school project where I need to have the NXT map a grid of 1x1ft squares. I urgently need help on this, as it is due in less than two weeks, and I can literally fail the course if I don't get it working (it's a major project). The major problem that I am having is with the Path finding algorithm, and the creation of LineMaps. Namely, I am having an extremely difficult time understanding how to setup and use the LineMaps. The documentation is virtually nonexistent, to make matters worse. I have referenced this post, but it was not helpful at resolving my issue: viewtopic.php?f=7&t=3223
In a nutshell, the robot needs to use the ultrasonic sensor to scan the grids that are in the front, and sides of the robot. This is working great. I have a 2D boolean array to store whether or not a grid location is or is not occupied.
I am using my algorithm to add a line map that can create lines to formulate the occupied grid square.
Virtually everything else with my project is working, except for this. I am currently getting exceptions indicating that the path cannot be found. Below are pertinent parts of my program.
Note: The 30.48f refers to the number of centimeters in a foot.
- Code: Select all
private static LineMap createLineMap()
// Temporarily store the lines in an ArrayList that will formulate the LineMap.
ArrayList<Line> lines = new ArrayList<Line>();
// Add each side of the rectangle [the bounding box].... (or square):
/*lines.add(new Line(0, 0, 0, (GRID_HEIGHT * 30.48f)));
lines.add(new Line(0, (GRID_HEIGHT * 30.48f), (GRID_WIDTH * 30.48f), (GRID_HEIGHT * 30.48f)));
lines.add(new Line((GRID_WIDTH * 30.48f), (GRID_HEIGHT * 30.48f), (GRID_WIDTH * 30.48f), 0));
lines.add(new Line((GRID_WIDTH * 30.48f), 0, 0, 0));
// The above is commented out...But, it has so far made not one bit of difference...
// I am wondering if the problem is that the point is actually on the obstacle line itself. If so, then
// that may cause an issue. One thing that I thought about was to add a buffer: add a buffer so that the point
// is in the *middle* of the cube.... But that didn't really help me either....
// For each obstacle (occupied square) that exists, need to create a line around that square:
for(int i = 0; i < GRID_WIDTH; i++)
for(int j = 0; j < GRID_HEIGHT; j++)
// If the position is occupied, add the lines to the line map:
// The left side:
lines.add(new Line(i * 30.48f, j * 30.48f, i * 30.48f, (j + 1) * 30.48f));
// The top side:
lines.add(new Line(i * 30.48f, (j + 1) * 30.48f, (i + 1) * 30.48f, (j + 1) * 30.48f));
// The right side:
lines.add(new Line((i + 1) * 30.48f, (j + 1) * 30.48f, (i + 1) * 30.48f, j * 30.48f));
// The bottom side:
lines.add(new Line((i + 1) * 30.48f, j * 30.48f, i * 30.48f, j * 30.48f));
// Next, create the bounding Rectangle:
Rectangle boundingRect = new Rectangle(0, 0, (GRID_WIDTH * 30.48f) + 5, (GRID_HEIGHT * 30.48f) + 5);
// Set the lineman:
lineMap = new LineMap((Line) lines.toArray(new Line[lines.size()]), boundingRect);
When I am ready to use the LineMap to navigate amongst the grid, here is the code I have: (the location is stored in a 2D int array that holds the x and y location in the grid of the robot)
- Code: Select all
DijkstraPathFinder pathFinder = new DijkstraPathFinder(lineMap);
// Get the current position for path generation:
Pose currentPosition = new Pose(location * 30.48f, location * 30.48f, direction.getCurrentHeading());
// Get the next waypoint
int nextWaypoint = getNextWaypoint();
Waypoint destination = new Waypoint(nextWaypoint * 30.48f, nextWaypoint * 30.48f);
// Generate the path:
Path path = pathFinder.findRoute(currentPosition, destination); // <--------- THIS IS WHAT THROWS THE ERROR! IT SAYS IT CAN'T ROUTE TO THAT LOCATION!
// Get the next spot to go:
Waypoint next = path.get(1);
I literally have NO idea what is wrong... I have previously tried using the FourWayGridMesh, along with the AstarSearchAlgorithm... This was working great for small 3x3 grids. But, larger grids cause it to fail. Basically, with the NodeSearhPath, the resuling path would say that the starting node for the current path is the origin node of the grid. Obviously, this causes serious issues. I peeked at the AstarSearchAlgorithm class's code, and it looks really buggy... Lot's of commented out lines. I am convinced that the AstarSearchAlgorithm is buggy, which is why I'm trying it with just the LineMap.
PLEASE PLEASE PLEASE PLEASE HELP!! I don't want to fail my course!! Time is running out for me to get this done.