void check_block(const Block *b) {
for (auto input : b->inputs()) {
check_value(input);
JIT_ASSERT(input->node()->kind_ == kParam);
}
for (auto n : b->nodes()) {
JIT_ASSERT(n->kind_ != kParam);
JIT_ASSERT(n->kind_ != kReturn);
check_node(n);
}
JIT_ASSERT(b->output_->kind() == kReturn);
check_node(b->output_);
// all_nodes
// - inputs_, output_ and nodes_ are all included in all_nodes
// - all_nodes does not contain dead nodes??? (likely to be temporarily
// suspended). Weaker: all_nodes contains all inputs and returns
// - only one return node???
node_set nodes_set(ALL_OF(b->nodes()));
node_set inputs_set {b->input_};
node_set output_set {b->output_};
// TODO: Make a more type safe std::includes wrapper which disallows use on
// non-ordered containers
JIT_ASSERT(std::includes(ALL_OF(all_nodes_set), ALL_OF(nodes_set)));
JIT_ASSERT(std::includes(ALL_OF(all_nodes_set), ALL_OF(inputs_set)));
JIT_ASSERT(std::includes(ALL_OF(all_nodes_set), ALL_OF(output_set)));
sum_set.insert(ALL_OF(nodes_set));
sum_set.insert(ALL_OF(inputs_set));
sum_set.insert(ALL_OF(output_set));
}
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;
}
