Hi, I’m trying to make a simple Pure Pursuit simulator using React JS and P5 JS, and I’m facing a problem right now with the fact that the program just stops working in some cases. For example, the path below does work:
incomplete post.
can’t help
2 Likes
Yeah, sorry about that, I pressed enter by mistake 
. So an example of a path that does not work is shown below:
In that image, the yellow and purple circles represent the intersection points (so basically the 2 purple points were the first intersections found and the two yellow points were the second one). Then the rectangle represents the area that the program is checking to see if the intersection points are within the correct area. But for some reason that part does not work. The code for finding the points is shown below:
import { lookAheadDistance } from "../App";
import { x as currentX } from "../App";
import { y as currentY } from "../App";
import { P5 as p5 } from "../App";
class FindGoalPoint {
// This function just returns either 1 or -1 based on whether or not the number is positive or negative
sgn(x) {
if (x >= 0) return 1;
else return -1;
}
// This function just finds all the of the points on the path that the robot is intersecting with
findIntersectionPoints(pointsHandler) {
let goalPt = null;
let sol1 = goalPt;
let sol2 = goalPt;
// let p1 = pointsHandler.Points[1];
// let p2 = pointsHandler.Points[2];
// let miX = Math.min(p1.x - currentX, p2.x - currentX);
// let maX = Math.max(p1.x - currentX, p2.x - currentX);
// let miY = Math.min(p1.y - currentY, p2.y - currentY);
// let maY = Math.max(p1.y - currentY, p2.y - currentY);
// p5.fill("green");
// p5.rect(miX + currentX, miY + currentY, maX + currentX, maY + currentY);
let colors = ["purple", "blue", "yellow", "white", "green"];
for (let i = 0; i < pointsHandler.Points.length - 1; ++i) {
let pointOne = pointsHandler.Points[i];
let pointTwo = pointsHandler.Points[i + 1];
console.log(`i is: ${i}`);
console.log(
`Point one x: ${pointOne.x}, Point one y: ${pointOne.y}`
);
console.log(
`Point two x: ${pointTwo.x}, Point two y: ${pointTwo.y}`
);
// This variables store the x and y values for both points (we are storing them in these variables to improve the readability of hte code)
let x1 = pointOne.x - currentX;
let y1 = pointOne.y - currentY;
let x2 = pointTwo.x - currentX;
let y2 = pointTwo.y - currentY;
// This calculates the variables needed for the line-circle-intersection
let dx = x2 - x1;
let dy = y2 - y1;
let dr = Math.sqrt(Math.pow(dx, 2) + Math.pow(dy, 2));
let D = x1 * y2 - x2 * y1;
// This calculates the discriminant
let discriminant =
Math.pow(lookAheadDistance, 2) * Math.pow(dr, 2) -
Math.pow(D, 2);
console.log(`Discriminant: ${discriminant}`);
// If the discriminant is less than 0, which means there are no solutions, then just continue in the loop
if (discriminant < 0 || pointOne == pointTwo) continue;
// This calculates the solutions for the x and y for both points
let x1Sol =
(D * dy + this.sgn(dy) * dx * Math.sqrt(discriminant)) /
Math.pow(dr, 2);
let x2Sol =
(D * dy - this.sgn(dy) * dx * Math.sqrt(discriminant)) /
Math.pow(dr, 2);
let y1Sol =
(-D * dx + Math.abs(dy) * Math.sqrt(discriminant)) /
Math.pow(dr, 2);
let y2Sol =
(-D * dx - Math.abs(dy) * Math.sqrt(discriminant)) /
Math.pow(dr, 2);
// These are the solutions after you add the currentX and currentY values back
sol1 = { x: x1Sol + currentX, y: y1Sol + currentY };
sol2 = { x: x2Sol + currentX, y: y2Sol + currentY };
// This finds the minimum and maximum values for the x and y coordinates between the two points which will be used later on for comparisons
let minX = Math.min(x1, x2);
let maxX = Math.max(x1, x2);
let minY = Math.min(y1, y2);
let maxY = Math.max(y1, y2);
console.log(`Min x: ${minX + currentX}, Max x: ${maxX + currentX}`);
console.log(`Min y: ${minY + currentY}, Max y: ${maxY + currentY}`);
console.log(`Solution 1 x: ${sol1.x}, Solution 1 y: ${sol1.y}`);
console.log(`Solution 2 x: ${sol2.x}, Solution 2 y: ${sol2.y}`);
p5.fill(colors[i]);
p5.rect(
minX + currentX,
minY + currentY,
maxX + currentX,
maxY + currentY
);
// Check if either of the solutions are within the min and max x and y values for both the points
let validPointOne =
minX <= x1Sol &&
x1Sol <= maxX &&
minY <= y1Sol &&
y1Sol <= maxY;
let validPointTwo =
minX <= x2Sol &&
x2Sol <= maxX &&
minY <= y2Sol &&
y2Sol <= maxY;
p5.fill(colors[i]);
p5.ellipse(sol1.x, sol1.y, 15, 15);
p5.ellipse(sol2.x, sol2.y, 15, 15);
// Check if either of the intersection points are valid
if (validPointOne || validPointTwo) {
// Check if the first intersection point is valid, if it is then set goal point to the first solution
if (validPointOne) goalPt = sol1;
// Check if the second intersection point is valid
if (validPointTwo) {
// Now check which one of the solutions is closer to the second point
if (
p5.dist(x2Sol, y2Sol, pointTwo.x, pointTwo.y) <
p5.dist(x1Sol, y1Sol, pointTwo.x, pointTwo.y)
) {
goalPt = sol2;
}
}
} else {
// If neither of the intersection points are valid then the robot may have deviated from the path so set the goal point to the first point
goalPt = { x: pointOne.x, y: pointOne.y };
p5.fill(colors[i]);
p5.ellipse(sol1.x, sol1.y, 15, 15);
p5.ellipse(sol2.x, sol2.y, 15, 15);
}
console.log(
`Goal point x is: ${goalPt.x}, Goal point y is: ${goalPt.y}`
);
}
console.log(
"////////////////////////////////////////////////////////////\n"
);
return goalPt;
}
}
export default FindGoalPoint;
If you have any questions about the code then I’m definitely happy to clarify! Thank you!
Nevermind, the problem was in the portion of the code shown below:
// Check if either of the intersection points are valid
if (validPointOne || validPointTwo) {
// Check if the first intersection point is valid, if it is then set goal point to the first solution
if (validPointOne) goalPt = sol1;
// Check if the second intersection point is valid
if (validPointTwo) {
// Now check which one of the solutions is closer to the second point
if (
p5.dist(x2Sol, y2Sol, pointTwo.x, pointTwo.y) <
p5.dist(x1Sol, y1Sol, pointTwo.x, pointTwo.y)
) {
goalPt = sol2;
}
}
}
Basically, the value of the goal point is null be default, and if the variable validPointOne is false then if the if statement inside of the if (validPointTwo) if statement evaluates to false then the goal point would stay null therefore crashing the program. So the solution to that was simply to add in the condition of goalPt === null so if the goal point is set to null it doesn’t matter what the distance is, it will still set goal point to the second solution.
2 Likes