hi, I’m having a certain problem with my code.
It’s a program with certain points (obstacles) and a robot which should find a path around it.
eventually the follower class will be a physical robot with a LiDAR sensor which sees the obstacles.
somehow the generated path does not always use the diagonals. what I’ve found is that is does when moving right to left, but not the other way around…
if someone could help me I’d appreciate it a lot!
(You can click to set a new endpoint)
code:
import java.util.*;
import pathfinder.*;
follower robot;
Graph graph;
GraphEdge[] edges;
GraphNode[] nodes;
GraphNode[] route;
IGraphSearch pathFinder;
GraphNode startNode, endNode, node;
int amount = 45; //amount of nodes per axis (preferably uneven amount so 'robot' is in the center)
float size = 20; //size of the spacing between the nodes (more = accurate but slower)
//total area = amount^2 * size
boolean newEndNode = true; //boolean to check if new endNode should be determined
int len = 360; //amount of contourpoint, in endapplication will be 1440. now 1 per degree
float[] x = new float[len]; //x location per contourpoint
float[] y = new float[len]; //y location per contourpoint
float[] angles = new float[len], distances = new float[len];
float minDist = (size*sqrt(2.0))/1.9; //minimum distance to check for closest node
//based on square with sides length "size", diagonal = size*sqrt(2), half of that. rounding errors caused a small gap, so instead of dividing by 2, divide by 1.9
void setup() {
size(1000, 1000); //create window of 1000 by 1000 pixels
robot = new follower(width/2,height/2);
//createGraph(); //create the graph based on amount and size
translate(width/2, height/2); //translate to the center of the window
strokeWeight(3); //set the strokeWeight (thickness of the lines) to 3
for (int i = 0; i < len; i++) { //for every contourpoint:
angles[i] = (i*TWO_PI/len); //determine the angle (degrees per point * current number of point)
//next are arbitrary angles and distances to create a view of contourpoints
if ( (angles[i] <= PI/3) || (angles[i] >= (6.5/4.0)*PI) ) {
distances[i] = 100/cos(angles[i])*2;
} else if ( (angles[i] > PI/3) && (angles[i] < PI/2) ) {
distances[i] = 100/sin(angles[i]);
} else if ( (angles[i] >= PI/2) && (angles[i] < TWO_PI/3) ) {
float a = map(angles[i]-PI/2, 0, (TWO_PI/3)-(PI/2), 100, 200);
distances[i] = a/cos(radians(i-90));
} else if ( (angles[i] >= TWO_PI/3) && (angles[i] < (4*PI)/3) ) {
distances[i] = -150*(sin(angles[i]/2)/cos(angles[i]));
} else {
distances[i] = angles[i] * 10;
}
x[i] = distances[i]*cos(angles[i]); //store contourpoints in x and y point
y[i] = distances[i]*sin(angles[i]);
//stroke(255,0,0);
//point(x[i], y[i]); //draw points
//if (graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist) != null) { //if there is a graphNode at current location:
// graph.removeNode(graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id()); //remove the node (creating a wall for pathfinder), also removes connecting edges
//}
}
// nodes = graph.getNodeArray(); //create node-array with all nodes left in the graph
// edges = graph.getAllEdgeArray(); //create edge-array with all edges left in the graph
// graph.compact(); //remove floating edges
}
void mousePressed() {
newEndNode = true; //allow new endnode on mouseclick
}
void draw() {
background(255); //white background
pathPlanner(); //custom path planning function (line 90)
drawPath(); //custom path drawing function
drawGraph(); //custom graph drawing function
if (endNode != null) { //if the endNode is not empty
node = graph.getNodeAt(endNode.xf(), endNode.yf(), 0, minDist); //closest node on the graph to the endNode location
if (node != null) { //if the node is not empty (there is a closest node)
//stroke(255, 0, 0); //color red
//ellipse(node.xf(), node.yf(), 10, 10); //draw a circle at node x and y with size 10
}
//stroke(0, 255, 0); //color green
//ellipse(endNode.xf(), endNode.yf(), 10, 10); //draw a circel at endNode x and y with size 10
//stroke(0); //color black
}
//float mx = mouseX-width/2; //translated mouse x
//float my = mouseY-height/2; //translated mouse y
robot.show();
//robot.update(route);
translate(width/2, height/2); //translate to the center of the window
strokeWeight(3); //set line thickness to 3
stroke(255,0,0);
for (int i = 0; i < len; i++) { //for every contourpoint:
point(x[i], y[i]); //draw contourpoint
//ellipse(mx, my, 10, 10); //draw circle at mouse position
}
}
void pathPlanner() { //custom path planner function
createGraph(); //create the graph with size and amount
for (int i = 0; i < len; i++) { //for every 'obstacle' point
if (graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist) != null) { //if there is a node
int id = graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id(); //get the id of the closest node
LinkedList<GraphEdge> edgeList1 = graph.getEdgeList(id+1); //node to the right
LinkedList<GraphEdge> edgeList2 = graph.getEdgeList(id-1); //node to the left
LinkedList<GraphEdge> edgeList3 = graph.getEdgeList(id+amount); //node underneath (1 row lower)
LinkedList<GraphEdge> edgeList4 = graph.getEdgeList(id-amount); //node above (1 row higher)
for (int k = edgeList1.size()-1; k >= 0; k--) { //for every edge connected to node to the right:
graph.addEdge(edgeList1.get(k).from().id(), edgeList1.get(k).to().id(), 400); //add edge with higher cost (400 instead of 100)
graph.removeEdge(edgeList1.get(k).from().id(), edgeList1.get(k).to().id()); //remove the older edge
}
for (int k = edgeList2.size()-1; k >= 0; k--) { //for every edge connected to node to the left
graph.addEdge(edgeList2.get(k).from().id(), edgeList2.get(k).to().id(), 400); //add edge with higher cost
graph.removeEdge(edgeList2.get(k).from().id(), edgeList2.get(k).to().id()); //remove old edge
}
for (int k = edgeList3.size()-1; k >= 0; k--) { //for every edge connected to node below
graph.addEdge(edgeList3.get(k).from().id(), edgeList3.get(k).to().id(), 400); //add edge with higher cost
graph.removeEdge(edgeList3.get(k).from().id(), edgeList3.get(k).to().id()); //remove old edge
}
for (int k = edgeList4.size()-1; k >= 0; k--) { //for every edge connected to node above
graph.addEdge(edgeList4.get(k).from().id(), edgeList4.get(k).to().id(), 400); //add edge with higher cost
graph.removeEdge(edgeList4.get(k).from().id(), edgeList4.get(k).to().id()); //remove old edge
}
graph.removeNode(id);//graph.getNodeAt(x[i]+width/2, y[i]+height/2, 0, minDist).id()); //remove node
}
}
startNode = graph.getNodeAt(robot.x, robot.y, 0, minDist); //startNode is at mouse position
graph.compact(); //remove floating edges
nodes = graph.getNodeArray(); //nodes array with all nodes from the graph
edges = graph.getAllEdgeArray(); //edges array with all edges from the graph
pathFinder = new GraphSearch_Astar(graph, new AshCrowFlight(1.0)); //set the pathfinder A* algoritm
if (newEndNode == true) { //if new endNode should be determined
int randomNode = floor(random(nodes.length-1)); //find random node-ID from the nodes array
node = nodes[randomNode]; //node is the new node
while(node.xf() > width || node.xf() < 0 || node.yf() > height || node.yf() < 0){
println("finding new node");
randomNode = floor(random(nodes.length-1));
node = nodes[randomNode];
}
//node = graph.getNodeAt(mouseX-width/2,mouseY-height/2,0,minDist);
endNode = node; //endNode is node
//println(endNode.xf(), endNode.yf());
newEndNode = false; //new endNode is determined, no new one neccessary
}
if (startNode != null && endNode != null && node != null) { //if all neccessary nodes are determined
pathFinder.search(startNode.id(), node.id()); //find the path between the startNode and endNode
route = pathFinder.getRoute(); //node-array route stores all nodes in the path
}
}
void createGraph() { //function to create the graph
graph = new Graph(); //create new graph
// Create and add node
GraphNode node; //create Graphnode
int node_id = 0; //start at node-id 0
int cost = 100; //cost = 100 (standard)
int deltaX = amount + 1;
for (int y = 0; y < amount; y++) { //for the amount on the y-axis
node_id = deltaX * y + deltaX;
for (int x = 0; x < amount; x++) { //for the amount on the x-axis
node = new GraphNode(node_id, (x-amount/2) * size + robot.x, (y-amount/2) * size + robot.y); //create node at location
graph.addNode(node); //add the node to the graph
if(x > 0){
graph.addEdge(node_id, node_id - 1, cost);
graph.addEdge(node_id, node_id - deltaX - 1, cost);
graph.addEdge(node_id, node_id + deltaX - 1, cost);
}
if(x < amount - 1){
graph.addEdge(node_id, node_id + 1, cost);
graph.addEdge(node_id, node_id - deltaX - 1, cost);
graph.addEdge(node_id, node_id + deltaX - 1, cost);
}
if(y > 0)
graph.addEdge(node_id, node_id - deltaX, cost);
if(y < amount - 1){
graph.addEdge(node_id, node_id + deltaX, cost);
}
node_id++;
}
}
}
void drawGraph() { //custom function to draw the graph
/* Edges first */
stroke(180, 180, 200); //lighter color
for (int i = 0; i < edges.length; i++) { //for every edge in the edges-array
GraphNode from = edges[i].from(); //get the startnode
GraphNode to = edges[i].to(); //get the endnode
int cost = int((float)(graph.getEdgeCost(from.id(), to.id()))); //get the cost from the edge between these nodes
strokeWeight(float(cost)/100.0); //thickness of the line depends on cost
line(from.xf(), from.yf(), to.xf(), to.yf()); //draw the edge
}
/* Nodes next */
stroke(0); //color black
strokeWeight(2); //set thickness to 2
for (int i = 0; i < nodes.length; i++) { //for every node in the nodes-array
GraphNode node = nodes[i]; //get the current node
//ellipse(node.xf(), node.yf(), 2, 2); //draw the node
//if (i%1000 == 0) //every 1000 nodes:
//text(node.id(), node.xf() + 4, node.yf() - 2); //draw text with node-id
}
}
void drawPath() { //custom function to draw the path
strokeWeight(3);
stroke(0,150,0);
noFill();
if (route != null && route.length != 0) { //if the route is not empty
//beginShape(); //start new shape
//curveVertex(route[0].xf(), route[0].yf()); //start curvevertex
for (int i = 1; i < route.length; i++) { //for every node in the route-array
//if (i % 2 == 0) //every 2 points:
//curveVertex(route[i].xf(), route[i].yf()); //add new points to smooth vertex
line(route[i-1].xf(), route[i-1].yf(), route[i].xf(), route[i].yf()); //draw route
}
//curveVertex(route[route.length-1].xf(), route[route.length-1].yf()); //add the last vertex
//curveVertex(route[route.length-1].xf(), route[route.length-1].yf()); //end the shape
//endShape(); //end the shape and draw the complete line
}
}
class follower { //visualize the 'robot'
float x,y; //position coordinates for robot
follower(float startX, float startY) { //given start position when initialised (see void setup())
x = startX; //position is start position
y = startY;
}
void update(GraphNode[] path) { //update function of the path following class
int current = 0; //current index of the path-array
if(path != null && path.length != 0 && path.length > 1){ //if the path is valid and the robot is not yet at the end
float dx = path[current+1].xf()-path[current].xf(); //calculate the difference between next x-position in path and current
float dy = path[current+1].yf()-path[current].yf(); //calculate the difference between next y-position in path and current
println(dx,dy); //print the diffences to console
x += dx*0.5; //add half of the diffence to x (0.5 determines speed, at full, it will oscillate at destination)
y += dy*0.5; //same as x
current++; //go to the next index of the path-array
if(path.length == 1){ //if the path only has 1 point (current position is the same as end position)
current = 0; //current index = 0, start new because path is finished
println("path finished"); //print to console
}
println("following path", dx, dy); //print to console, it's still following with current differences
}else println("path finished"); //print end of the path has been reached
}
void show() { //show function of the follower class
stroke(0,0,255); //color blue
strokeWeight(3); //thickness 3
fill(0,0,255); //fill color blue
ellipse(x,y,2,2); //draw circle with diameter 2 at position
}
}
also the vertexes in drawPath are used to smoothen the line drawn for a more pleasent look, but right now that’s commented out to show the actual path