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";
}
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
}
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;
}
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;
}
