コード例 #1
0
void MazeWalker::walkMaze(){

	if(m_isDFS){
		while(!isGoalReached()){
			storeValidMoves();

			if(m_moveStack.empty()){
				m_noExit = true;
				std::cout << "\nNo exit\n\n";
			}
			else{
				move(m_moveStack.top());
				m_moveStack.pop();
			}
		}
	}

	else{
		while(!isGoalReached()){
			storeValidMoves();

			if(m_moveQueue.empty()){
				m_noExit = true;
				std::cout << "\nNo exit\n\n";
			}
			else{
				move(m_moveQueue.front());
				m_moveQueue.pop();
			}
		}
	}

	if(!m_noExit)
		std::cout << "\nExit Found!\n\n";
}
コード例 #2
0
ファイル: MazeWalker.cpp プロジェクト: dravidjoseph/School
bool MazeWalker::walkMaze(){
	
	//Do DFS
	if(m_searchType == Search::DFS){
		//Load starting position into stack
		m_moveStack.push(m_startPos);
		
		//keep this going until either the stack is empty or the goal is reached
		while(!(m_moveStack.empty() || isGoalReached())){
			//store valid moves from start or last iteration
			storeValidMoves();
			//move m_curPos to the top of stack 
			move(m_moveStack.top());
			
			//pop the position from the stack
			m_moveStack.pop();
			
		}	
	}
	else{
		//Load starting position into queue
		m_moveQueue.push(m_startPos);
		
		//keep going until queue is empty or goal is reached
		while(!(m_moveQueue.empty() || isGoalReached())){
			
			//store valid moves at the end of the queue
			storeValidMoves();
			//Remove from the front of the queue
  		  	m_moveQueue.pop();
			
			//Ensures that once the queue is empty no moves will be made.
			if(!m_moveQueue.empty()){
				move(m_moveQueue.front());
			}
		}
	}
	return isGoalReached();		//either it's reached or it isn't
}
コード例 #3
0
ファイル: MazeWalker.cpp プロジェクト: hungtn95/EECS_268
void MazeWalker::walkMaze()
{
	move(*m_startPos);
	mazePrint();
	while (!isGoalReached())
	{
		storeValidMoves();
		if (m_isDFS)
		{
			if (m_moveStack.empty())
			{
				break;
			}
			move(m_moveStack.top());
			m_moveStack.pop();
		}
		else
		{
			if (m_moveQueue.empty())
			{
				break;
			}
			move(m_moveQueue.front());
			m_moveQueue.pop();
		}
		mazePrint();
	}
	if (isGoalReached())
	{
		std::cout << '\n' << "Exit found!" << '\n';
	}
	else
	{
		std::cout << '\n' << "No way out!" << '\n';
	}
}
//reaching the target controller - FIRST CONTROLLER
icarsCarControlWebsocket::SimRefs control_commands( geometry_msgs::Twist corrected_control)
{
 geometry_msgs::Pose theoretical_pose, temp_pose_1;

  int i, j, k, ped_grid_i, ped_grid_j;
  int arg_vel_obs=0;
  int max_i, min_i;
  int max_j, min_j;

double error = isGoalReached(); // ERROR W.r.t TO FINAL GOAL

if (corrected_control.angular.z!=0 && corrected_control.linear.x!=0){             

for(k=0;k<nbMap;k++){

   temp_pose_1.position.x = robot_pose.x + k*timeStep*arg_vel_obs*cos(robot_pose.theta);
   temp_pose_1.position.y = robot_pose.y + k*timeStep*arg_vel_obs*cos(robot_pose.theta);

   ped_grid_i = gridIFromPose(temp_pose_1);
   ped_grid_j = gridJFromPose(temp_pose_1);

    min_i = ped_grid_i - 2;
    max_i = ped_grid_i + 2;
    min_j = ped_grid_j - 2;
    max_j = ped_grid_j + 2;

     for(i=min_i ; i<=max_i ; i++){
      for(j=min_j ; j<=max_j ; j++){

// PART 1: BRAKE IF OBSTACLE DETECTED 
        if(og_array.array[k].data[gridIndexFromCoord(i,j)] != 0)
        {
        corrected_control = brake();
        ROS_INFO("OBSTACLE DETECTED AHEAD");
        obstacle_detected = true;
        }

      }
        }
}

if(obstacle_detected == false)
 {
// PART 2: BRAKE IF GOAL REACHED
if (error < 1.5 && (msg_ctr.brakeStrength!=100.0))
{
 corrected_control = brake();
 ROS_INFO("GOAL REACHED");
 goal_reached = true;
 } 

// PART 3: SENDING REQUIRED VELOCITY COMMANDS TO REACH THE GOAL
if(goal_reached == false){
    msg_ctr.steerAng=corrected_control.angular.z;
    msg_ctr.brakeStrength=0.0;
    msg_ctr.gasForce=corrected_control.linear.x;
     }
 }

} 
    
else{
        corrected_control = brake();
        ROS_INFO("WAITING FOR GRID MAP INFO");
    }

return msg_ctr;

}
コード例 #5
0
ファイル: MazeWalker.cpp プロジェクト: KueiHsienChu/EECS_268
bool MazeWalker::walkMaze()
{
	 
	/*
	set up position; 
	beware that value may exceed or less than the m_rows and m_cols, 
	so we need to set conditions to check
	*/
	
	char up_ = 'n'; char right_ = 'n'; char down_ = 'n'; char left_ = 'n';
	
	//direction_ is the character at the specific location w.r.t the m_curPos to the maze 
	//So the first time this method is called, it's at the start position
	if(m_curPos.getRow()-1 >= 0)
		up_ = m_maze[ m_curPos.getRow()-1 ][ m_curPos.getCol() ];
	if(m_curPos.getCol()+1 < m_cols)
		right_ = m_maze[ m_curPos.getRow() ][ m_curPos.getCol()+1 ];
	if(m_curPos.getRow()+1 < m_rows)
		down_ = m_maze[ m_curPos.getRow()+1 ][ m_curPos.getCol() ];
	if(m_curPos.getCol()-1 >= 0)
		left_ = m_maze[ m_curPos.getRow() ][ m_curPos.getCol()-1 ];
		
	//positions ---- use to check if they exceeds the boundary of the maze
	int up_move = m_curPos.getRow()-1;
	int right_move = m_curPos.getCol()+1;
	int down_move = m_curPos.getRow()+1;
	int left_move = m_curPos.getCol()-1;


	if(m_searchType == Search::DFS)
	{
	
		
		if(isGoalReached())
		{
			while(m_moveStack.size() > 0)
				m_moveStack.pop();
			return true;
		}
		else
		{	
			if((up_ == 'P' || up_ == 'E') && up_move >= 0) //so it's within range
			{
				Position up = Position( m_curPos.getRow()-1 , m_curPos.getCol() );
				m_moveStack.push(up);
			}
			
			if((right_ == 'P' || right_ == 'E') && right_move < m_cols) //so it's within range
			{
				Position right = Position( m_curPos.getRow() , m_curPos.getCol()+1 );
				m_moveStack.push(right);
			}
		
			if((down_ == 'P' || down_ == 'E') && down_move < m_rows)  //so it's within range
			{
				Position down = Position( m_curPos.getRow()+1 , m_curPos.getCol() );
				m_moveStack.push(down);
			}
		
			if((left_ == 'P' || left_ == 'E') && left_move >= 0 )  //so it's within range
			{
				Position left = Position( m_curPos.getRow() , m_curPos.getCol()-1 );
				m_moveStack.push(left);
			}
			while(m_moveStack.size() > 0)
			{	
				Position temp = Position(m_moveStack.top().getRow(), m_moveStack.top().getCol());
				m_moveStack.pop(); //pop
				if( m_visited[ temp.getRow() ][ temp.getCol() ] == 0 )
				{	move(temp); //move to that temporary position
					storeValidMoves(); //mark as visited
					walkMaze();
				}
				
			}//endwhile
		}//end else	
	}//end if DFS	
	
	if(m_searchType == Search::BFS)
	{
		m_moveQueue.push(m_curPos); //EnQueue( m.StartNode )
			
		while(m_moveQueue.size() > 0)
		{
			
			m_curPos = Position(m_moveQueue.front().getRow(), m_moveQueue.front().getCol());
			
			//store valid moves after first run
			if(m_curPos.getRow() != m_startPos.getRow() || m_curPos.getCol() != m_startPos.getCol())
				storeValidMoves();
			
			//conditions	
			if(m_curPos.getRow()-1 >= 0)
				up_ = m_maze[ m_curPos.getRow()-1 ][ m_curPos.getCol() ];
			else
				up_ = 'n';
			if(m_curPos.getCol()+1 < m_cols)
				right_ = m_maze[ m_curPos.getRow() ][ m_curPos.getCol()+1 ];
			else
				right_ = 'n';
			if(m_curPos.getRow()+1 < m_rows)
				down_ = m_maze[ m_curPos.getRow()+1 ][ m_curPos.getCol() ];
			else
				down_ = 'n';
			if(m_curPos.getCol()-1 >= 0)
				left_ = m_maze[ m_curPos.getRow() ][ m_curPos.getCol()-1 ];
			else
				left_ = 'n';
			
			int up_move = m_curPos.getRow()-1;
			int right_move = m_curPos.getCol()+1;
			int down_move = m_curPos.getRow()+1;
			int left_move = m_curPos.getCol()-1;
			
			m_moveQueue.pop();
			
			if(isGoalReached())
			{
				while(m_moveQueue.size() > 0)
				{
					m_moveQueue.pop();
				}
				return true;
			}	
			else
			{	
				//first, we need to store all valid neighbors into the queue
				//for each neighbor: up - right - down - left
				if((up_ == 'P' || up_ == 'E') && up_move >= 0) //so it's within range
				{
					//create a position
					Position up = Position( m_curPos.getRow()-1 , m_curPos.getCol() );
					//push to quene
					if( m_visited[ up.getRow() ][ up.getCol() ] == 0 )
						m_moveQueue.push(up);
				}
			
				if((right_ == 'P' || right_ == 'E') && right_move < m_cols) //so it's within range
				{
					Position right = Position( m_curPos.getRow() , m_curPos.getCol()+1 );
					if( m_visited[ right.getRow() ][ right.getCol() ] == 0 )
						m_moveQueue.push(right);
				}
		
				if((down_ == 'P' || down_ == 'E') && down_move < m_rows)  //so it's within range
				{
					Position down = Position( m_curPos.getRow()+1 , m_curPos.getCol() );
					if( m_visited[ down.getRow() ][ down.getCol() ] == 0 )
						m_moveQueue.push(down);
				}
		
				if((left_ == 'P' || left_ == 'E') && left_move >= 0 )  //so it's within range
				{
					Position left = Position( m_curPos.getRow() , m_curPos.getCol()-1 );
					if( m_visited[ left.getRow() ][ left.getCol() ] == 0 )
						m_moveQueue.push(left);			
				}
				
			}//end else
		}//end while
			
	}
	return false;
	
}