コード例 #1
0
ファイル: ir.cpp プロジェクト: MaheshBhosale/pytorch
    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));
    }
コード例 #2
0
ファイル: path_planner.cpp プロジェクト: TheiaRobo/Theia
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;



}