void PathPlanner::fill_g_f(cell cell_from){
int i,j;
double currentGval;
bool in_open_list = false;
_close_list.insert(cell_from);
for(i=cell_from.y_Coordinate-1; i <=cell_from.y_Coordinate+1; i++)
{
for(j=cell_from.x_Coordinate-1; j <= cell_from.x_Coordinate+1; j++)
{
if((i>=0)&&(j>=0)&&(i<_grid.size())&&(j<_grid[0].size()))
{
if (i == _goal.y_Coordinate && j == _goal.x_Coordinate )
{
cell cl2(j, i);
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
_grid[i][j].g_val = 0;
_grid[i][j].f_val = 0;
_open_list.insert(cl2);
}
else {
bool is_close = check_in_set(_close_list,i,j);
if((_grid[i][j].cell_color == 0)&&(((j != cell_from.x_Coordinate)||(i != cell_from.y_Coordinate)))&&(!is_close))
{
cell cl(j, i);
currentGval = g_cost(cell_from, cl);
in_open_list = check_in_set(_open_list,i,j);
if(in_open_list){
if (_grid[i][j].g_val > currentGval){
_grid[i][j].g_val = currentGval;
_grid[i][j].f_val = _grid[i][j].g_val + _grid[i][j].h_val;
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
}
}
else{
_grid[i][j].g_val = currentGval;
_grid[i][j].f_val = _grid[i][j].g_val + _grid[i][j].h_val;
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
_open_list.insert(cl);
}
}
}
}
}
}
}
std::vector<node> find_closest(int x_i, int y_i, std::vector<signed char> matrix_array, int val_to_find){
int lateral_size=std::sqrt(matrix_array.size());
std::vector<std::vector<signed char> >matrix(lateral_size);
int goal_coords[2]={NO_VAL,NO_VAL},coords[2],from[2],cost=0;
std::vector<node> return_error;
double t_f=0, t_g=0, t_h=0;
// Conversion to matrix
for(int i=0; i < lateral_size; i++){
matrix[i].resize(lateral_size,blue);
}
for(int x=0; x < lateral_size; x++){
for(int y=0; y < lateral_size; y++){
matrix[x][y]=matrix_array[x+y*lateral_size];
}
}
if(x_i < 0 || y_i < 0 || x_i >= lateral_size || y_i >= lateral_size){
ROS_ERROR("Invalid initial coordinates");
return return_error;
}
// values different from white and val_to_find are obstacles
if(val_to_find==-1){ // back to start
ROS_WARN("Will plan a path back to the start");
goal_coords[0]=lateral_size/2;
goal_coords[1]=lateral_size/2;
ROS_WARN("My goal is set to the start position: (%d,%d)",goal_coords[0],goal_coords[1]);
}else{
for(int x=0; x < lateral_size; x++){
for(int y=0; y < lateral_size; y++){
if(matrix[x][y]!=white && matrix[x][y]!=val_to_find){
matrix[x][y]=black;
}else if(val_to_find!=gray && matrix[x][y]==val_to_find){
goal_coords[0]=x;
goal_coords[1]=y;
}
}
}
}
if(val_to_find==gray){ // inefficient, but better than running A* without an heuristic
for(int i=0; i < lateral_size/2-1 && goal_coords[0]==NO_VAL;i++){
for(int x=x_i-i/2; x < x_i+i/2;x++){
for(int y=y_i-i/2; y<y_i+i/2;y++){
if(matrix[x][y]==gray){
goal_coords[0]=x;
goal_coords[1]=y;
}
}
}
}
}
ROS_WARN("My goal is set to: (%d,%d)",goal_coords[0],goal_coords[1]);
// A*
node current,n;
coords[0]=x_i;
coords[1]=y_i;
from[0]=NO_VAL;
from[1]=NO_VAL;
t_g=0;
t_h=heur(coords,goal_coords);
t_f = t_g + t_h;
current=create_node(coords,t_f,t_g,from);
return_error.push_back(current);
if(goal_coords[0]==NO_VAL){
ROS_INFO("No solution exists");
return return_error;
}
search_set closedset;
search_set openset(current);
search_set came_from;
search_set nodes_set(current);
int i=0;
signal(SIGINT, &trap);
while(!openset.isempty() && execute){
i++;
//ROS_INFO("iter: %d",i);
current=openset.pop_best();
/*ROS_INFO("Will evaluate node (%d,%d).",current.coords[0],current.coords[1]);
ROS_INFO(" t_f = %.2f, t_g = %.2f",current.t_f,current.t_g);
getchar();*/
if(current.coords[0]==goal_coords[0] && current.coords[1]==goal_coords[1]){
ROS_INFO("Found solution at %d %d!",current.coords[0],current.coords[1]);
return retrieve_path(current, &closedset);
}
closedset.push_node(current);
// FOR EACH NEIGHBOUR
if(current.coords[0]-1>=0){
coords[0]=current.coords[0]-1;
coords[1]=current.coords[1];
if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){
t_h=heur(n.coords,goal_coords);
t_g = current.t_g + g_cost(coords,current.coords,current.came_from);
t_f = t_g + t_h;
if(openset.check_if_in_set(coords)){
n = openset.read_node(coords); // only doing this to get the f value, if present
}else{
n = create_node(coords,t_f,t_g,current.coords);
}
if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){
n.t_g=t_g;
n.t_f=t_f;
n.came_from[0]=current.coords[0];
n.came_from[1]=current.coords[1];
if(!openset.check_if_in_set(n.coords))
openset.push_node(n);
else{ // update the value of the node in openset
openset.pop_requested(n.coords);
openset.push_node(n);
}
}
}
}
if(current.coords[0]+1<lateral_size){
coords[0]=current.coords[0]+1;
coords[1]=current.coords[1];
if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){
t_h=heur(n.coords,goal_coords);
t_g = current.t_g + g_cost(coords,current.coords,current.came_from);
t_f = t_g + t_h;
if(openset.check_if_in_set(coords)){
n = openset.read_node(coords);
}else{
n = create_node(coords,t_f,t_g,current.coords);
}
if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){
n.t_g=t_g;
n.t_f=t_f;
n.came_from[0]=current.coords[0];
n.came_from[1]=current.coords[1];
if(!openset.check_if_in_set(n.coords))
openset.push_node(n);
else{
openset.pop_requested(n.coords);
openset.push_node(n);
}
}
}
}
if(current.coords[1]-1>=0){
coords[0]=current.coords[0];
coords[1]=current.coords[1]-1;
if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){
t_h=heur(n.coords,goal_coords);
t_g = current.t_g + g_cost(coords,current.coords,current.came_from);
t_f = t_g + t_h;
if(openset.check_if_in_set(coords)){
n = openset.read_node(coords);
}else{
n = create_node(coords,t_f,t_g,current.coords);
}
if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){
n.t_g=t_g;
n.t_f=t_f;
n.came_from[0]=current.coords[0];
n.came_from[1]=current.coords[1];
if(!openset.check_if_in_set(n.coords))
openset.push_node(n);
else{
openset.pop_requested(n.coords);
openset.push_node(n);
}
}
}
}
if(current.coords[1]+1<lateral_size){
coords[0]=current.coords[0];
coords[1]=current.coords[1]+1;
if(matrix[coords[0]][coords[1]] == white || matrix[coords[0]][coords[1]] == val_to_find){
t_h=heur(n.coords,goal_coords);
t_g = current.t_g + g_cost(coords,current.coords,current.came_from);
t_f = t_g + t_h;
if(openset.check_if_in_set(coords)){
n = openset.read_node(coords);
}else{
n = create_node(coords,t_f,t_g,current.coords);
}
if(!closedset.check_if_in_set(n.coords) || t_f < n.t_f ){
n.t_g=t_g;
n.t_f=t_f;
n.came_from[0]=current.coords[0];
n.came_from[1]=current.coords[1];
if(!openset.check_if_in_set(n.coords))
openset.push_node(n);
else{
openset.pop_requested(n.coords);
openset.push_node(n);
}
}
}
}
}
ROS_INFO("Did not find anything :(");
return return_error;
}
