int main(int argc, char *argv[])
{
vector<string>ground;
getInput(ground);
detectDeadSquare(ground);
//showGround(ground);
/*
puts("1) Breadth first search");
puts("2) Depth first search");
puts("3) Uniform cost search");
puts("4) Greedy best first search");
puts("5) A* search");
*/
freopen("output.txt", "w", stdout);
int choice;
sscanf(argv[argc - 1], "%d", &choice);
switch(choice) {
case 1: BFS(ground); break;
case 2: DFS(ground); break;
case 3: UCS(ground); break;
case 4: GFS(ground); break;
case 5: AStar(ground); break;
}
return 0;
}
int main()
{
std::vector< Coord > path;
Point *start = new Point( 5, 1, 0, nullptr );
Point *end = new Point( 5, 15, 0, nullptr );
glfwInit();
DrawPath( path );
GLFWwindow *wind = glfwCreateWindow( 800, 600, "A* Pathfinding", nullptr, nullptr );
glfwMakeContextCurrent( wind );
Reshape( wind, 800, 600 );
glfwSetWindowSizeCallback( wind, Reshape );
glfwSetCursorPosCallback( wind, mousePosCallBack );
glfwSetMouseButtonCallback( wind, mouseClickCallBack );
srand( time(nullptr) );
for( int y = 0; y < walls.size(); y++ )
{
for( int x = 0; x < walls[y].size(); x++ )
{
walls[x][y] = 1;
}
}
while( !glfwWindowShouldClose( wind ) )
{
glClear( GL_COLOR_BUFFER_BIT );
glLoadIdentity();
glTranslatef( 0.0f, 0.0f, -1.0f );
path = AStar( walls, *start, *end );
DrawMap();
DrawPath( path );
std::this_thread::sleep_for( std::chrono::milliseconds(17) );
//ResetKeys();
glfwPollEvents();
glfwSwapBuffers( wind );
}
glfwTerminate();
}
void Benchmark::calculate(Implementation Type) {
switch(Type){
case bfs:
BFS(&benchGraph, rand() % Vertices, rand() % Vertices);
break;
case dfs:
DFS(&benchGraph, rand() % Vertices, rand() % Vertices);
break;
case astar:
AStar(&benchGraph, rand() % Vertices, rand() % Vertices, Side);
break;
}
}
//--------------------------- CreatePathAStar ---------------------------
//------------------------------------------------------------------------
void BattleMap::createPathAStar()
{
//set current algorithm
m_CurrentAlgorithm = AT_ASTAR;
//create and start a timer
PrecisionTimer timer; timer.Start();
//create a couple of typedefs so the code will sit comfortably on the page
typedef Graph_SearchAStar<NavGraph, Heuristic_Euclid> AStarSearch;
//create an instance of the A* search using the Euclidean heuristic
AStarSearch AStar(*m_pGraph, m_iSourceCell, m_iTargetCell);
//record the time taken
m_dTimeTaken = timer.TimeElapsed();
m_Path = AStar.GetPathToTarget();
//m_SubTree = AStar.GetSPT();
m_dCostToTarget = AStar.GetCostToTarget();
}
int main() {
int n, t, row, x;
int i, j;
while(scanf("%d %d", &R, &C) == 2) {
if(R == 0 && C == 0) break;
scanf("%d", &n);
memset(map, 0, sizeof(map));
for(i = 0; i < n; i++) {
scanf("%d %d", &row, &t);
while(t--) {
scanf("%d", &x);
map[row][x] = -1;
}
}
scanf("%d %d %d %d", &sx, &sy, &ex, &ey);
AStar();
}
return 0;
}
std::vector<State*> SearchTree::doSearch(std::string algorithm)
{
double startT, finishT;
#ifdef __unix
timespec startWallTime, finishWallTime;
clock_gettime(CLOCK_MONOTONIC, &startWallTime);
#else
GET_TIME(startT);
#endif
std::clock_t startCpuTime = std::clock();
if (algorithm == "bcktrk")
solution = backTracking(root);
else if (algorithm == "dfs")
solution = depthFirstSearch(dfsDepthLimit);
else if (algorithm == "bfs")
solution = breadthFirstSearch();
else if (algorithm == "ucs")
solution = orderSearch();
else if (algorithm == "greedy")
solution = greedy();
else if (algorithm == "astr")
solution = AStar();
else if (algorithm == "idastr")
solution = IDAStar();
searchCpuTime = (std::clock() - startCpuTime) / (double)CLOCKS_PER_SEC;
#ifdef __unix
clock_gettime(CLOCK_MONOTONIC, &finishWallTime);
searchWallTime = (finishWallTime.tv_sec - startWallTime.tv_sec) + (finishWallTime.tv_nsec - startWallTime.tv_nsec) / 1000000000.0;
#else
GET_TIME(finishT);
searchWallTime = finishT - startT;
#endif
return getPathTo(solution);
}
int main(int argc, char **argv)
{
int i = 0, j;
int end_flag = -1;
static int count = 0;
geometry_msgs::PoseStamped posestamped;
nav_msgs::GetMap getmap;
ros::init(argc, argv, "astar_navi");
ros::NodeHandle n;
ros::Subscriber map_sub = n.subscribe("/map", 1000, mapCallback);
ros::Subscriber goal_sub = n.subscribe("/move_base_simple/goal", 1000, GoalCallback);
ros::Subscriber start_sub = n.subscribe("/initialpose", 1000, StartCallback);
ros::Publisher lane_pub = n.advertise<nav_msgs::Path>("lane_waypoint", 1000, true);
ros::Publisher ruled_pub = n.advertise<waypoint_follower::lane>("traffic_waypoint", 1000, true);
ros::Publisher pose_pub = n.advertise<geometry_msgs::PoseArray>("poses", 1000, true);
ros::Publisher start_pub = n.advertise<geometry_msgs::PoseStamped>("start_pose", 1000, true);
ros::Publisher footprint_pub = n.advertise<geometry_msgs::PolygonStamped>("car_footprint", 1000, true);
ros::ServiceClient getmap_srv_ = n.serviceClient<nav_msgs::GetMap>("/static_map");
plan_poses.header.frame_id = PATH_FRAME;//for debug
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok()){
ros::spinOnce();
if (_map_set == false || _goal_set == false || _start_set == false) {
std::cout << "\rtopic waiting \rtopic waiting";
for (j = 0; j < i; j++) {std::cout << ".";}
i++;
i = i%10;
std::cout << std::flush;
loop_rate.sleep();
continue;
}
/*******for debug***********/
/*
sx = 58; sy = 128;
dstheta = 0.000;
stheta = 0;
map[sy][sx][stheta].x = 5.899; map[sy][sx][stheta].y = 12.898;
map[sy][sx][stheta].theta = 0.00;
*/
/***************************/
/* Atar search loop */
while(1){
count++;
end_flag = AStar();
if (!(count%1000) || count < 1000){
pose_pub.publish(plan_poses);//for visualization
}
if (!end_flag){
std::cout << "GOAL!!" << std::endl;
PrintPath();
lane_pub.publish(plan_path);
ruled_pub.publish(ruled_waypoint);
pose_pub.publish(plan_poses);
Carfootprint();
_start_set = false;
_goal_set = false;
_map_set = false;
/* reset map data */
InitData();
getmap_srv_.call(getmap);
mapReset(getmap.response.map);
break;
} else if (end_flag == 1){
std::cout << "FAILED..." << std::endl;
_start_set = false;
_goal_set = false;
_map_set = false;
/* reset map data */
InitData();
getmap_srv_.call(getmap);
mapReset(getmap.response.map);
break;
} else {
continue;
}
}
}
return 0;
}
void run()
{
ros::Rate loop_rate(50);
while(ros::ok())
{
ros::spinOnce();
if(pos_set)
break;
loop_rate.sleep();
}
Pose waypoint = current_pos;
std::vector<Point> path;
Twist cmd_vel;
while(ros::ok()) {
messageFiller();
messagePub.publish(msg);
std::cout << "Publishing" << std::endl;
while(!obstacles_set || !pos_set || !destination_set) {
std::cout << "Waiting for obstacle list" << std::endl;
ros::spinOnce();
loop_rate.sleep();
}
std::cout << "Reached here" << std::endl;
if(obstacles_updated) { //find new path
long destination_x = std::lround((destination_pos.position.x + X_MAX / 2) * SCALE);
long destination_y = std::lround((destination_pos.position.y + Y_MAX / 2) * SCALE);
if(destination_x < 0) destination_x = 0;
if(destination_y < 0) destination_y = 0;
if(destination_x >= X_MAX * SCALE) destination_x = X_MAX * SCALE - 1;
if(destination_y >= Y_MAX * SCALE) destination_y = Y_MAX * SCALE - 1;
long current_x = std::lround((current_pos.position.x + X_MAX / 2) * SCALE);
long current_y = std::lround((current_pos.position.y + Y_MAX / 2) * SCALE);
if(current_x < 0) current_x = 0;
if(current_y < 0) current_y = 0;
if(current_x >= X_MAX * SCALE) current_x = X_MAX * SCALE - 1;
if(current_y >= Y_MAX * SCALE) current_y = Y_MAX * SCALE - 1;
path = AStar(map, X_MAX, Y_MAX, SCALE, Point(current_x, current_y), Point(destination_x, destination_y));
//waypoint = current_pos;
printMap(map, X_MAX, Y_MAX, SCALE);
obstacles_updated = false;
}
//reached waypoint
if (!path.empty()) { //next waypoint available
if(current_pos.position.x > waypoint.position.x - TOL &&
current_pos.position.x < waypoint.position.x + TOL &&
current_pos.position.y > waypoint.position.y - TOL &&
current_pos.position.y < waypoint.position.y + TOL) {
path.pop_back();
Point target = path.back();
waypoint.position.x = (target.first / SCALE) - (X_MAX / 2);
waypoint.position.y = (target.second / SCALE) - (Y_MAX / 2);
// if(!path.empty()) { //might make things smoother
// Point next = path.back();
// double angle = normalizeAngle(atan2(next.second - target.second, next.first - target.first));
// waypoint.orientation.z = sin(angle/2);
// waypoint.orientation.w = cos(angle/2);
// }
}
}
else //reached destination
{
std::cout << "Path empty" << std::endl;
cmd_vel.angular.z = 0;
cmd_vel.linear.x = 0;
velPub.publish(cmd_vel);
break;
}
calculate_u_omega(current_pos, waypoint, cmd_vel);
velPub.publish(cmd_vel);
ros::spinOnce();
loop_rate.sleep();
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "eser4");
ros::NodeHandle n;
ros::Subscriber read_position, read_laser;
goal_x = atof(argv[1]);
goal_y = atof(argv[2]);
letturaMappa("/home/tonish/Desktop/mappa.pgm");
aggiornaMappaVertici();
salvaMappa("/home/tonish/Desktop/vertici.pgm", true);
read_position = n.subscribe("/base_pose_ground_truth", 10, subPose);
read_laser = n.subscribe("/base_scan", 10, laserCallBack);
ros::Publisher twist_pub;
geometry_msgs::Twist msg;
twist_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
p_node path = NULL;
p_node best_node = NULL;
p_node final_goal = NULL;
final_goal = insertNode(final_goal, (int)round(fabs(3*goal_x)), (int)round(fabs(3*goal_y)), 0, NULL);
ros::Rate loop_rate(10);
while(check)
ros::spinOnce();
while(ros::ok()) {
if (fabs(robot_pose_x-goal_x) < 0.5 && fabs(robot_pose_y-goal_y) < 0.5) {
printf("GOAL RAGGIUNTO!!\n");
ros::shutdown();
}
if (robot_pose_orientation > M_PI)
robot_pose_orientation -= 2*M_PI;
if (f_tot < 100) {
msg.linear.x = LINEAR_V;
msg.angular.z = 0.0;
} else {
if (linear_movement) {
msg.linear.x = LINEAR_V;
msg.angular.z = 0.0;
}
if (angular_movement_left) {
msg.linear.x = 0.0;
msg.angular.z = ANGULAR_V;
}
if (angular_movement_right) {
msg.linear.x = 0.0;
msg.angular.z = -ANGULAR_V;
}
}
if (stop && one_time) {
msg.linear.x = 0.0;
msg.angular.z = 0.0;
one_time = false;
}
if (stop) {
p_node temp = AStar();
path = insertInHead(path, final_goal);
while (temp->parent) {
path = insertInHead(path, temp);
temp = temp->parent;
}
goal_f_x = undiscretize(-path->x);
goal_f_y = undiscretize(path->y);
printf("coordinate del primo vertice= x:%f, y:%f \n", goal_f_x, goal_f_y);
stop = false;
go = true;
control = false;
}
if (fabs(robot_pose_x-goal_f_x) < 0.1 && fabs(robot_pose_y-goal_f_y) < 0.1) {
if (path->next) {
path = path->next;
goal_f_x = undiscretize(-path->x);
goal_f_y = undiscretize(path->y);
printf("coordinate del vertice corrente= x:%f, y:%f \n", goal_f_x, goal_f_y);
}
}
twist_pub.publish(msg);
loop_rate.sleep();
ros::spinOnce();
}
}
void Maze::update( sf::RenderWindow &window )
{
if( !done_generating_ )
{
if( auto_generate_ )
{
//reset the maze the first run
if( first_generate_run_ )
{
for( int i = 0; i < height_ * width_; ++i)
{
maze_[i].setState( MazeElement::Wall );
}
maker_ = MazeMaker( maze_, width_, height_, width_ / 2, height_ / 2 );
first_generate_run_ = false;
}
done_generating_ = !maker_.update( maze_ );
if( done_generating_ )
{
start_x_ = maker_.getStartX();
start_y_ = maker_.getStartY();
stop_x_ = maker_.getStopX();
stop_y_ = maker_.getStopY();
}
}
if( sf::Mouse::isButtonPressed( sf::Mouse::Left ) )
{
//if the mouse button is being pressed, we want to draw
//so get the mouse position
sf::Vector2i mouse = sf::Mouse::getPosition( window );
//convert the mouse position to an x and y coordinate in our
//array of MazeElements
int x = mouse.x / MazeElement::getWidth();
int y = mouse.y / MazeElement::getHeight();
//It is possible for the mouse to stay clicked as it exits the screen
//so make sure we only draw to indexable array elements
if( x < width_ && y < height_ &&
x > 0 && y > 0)
{
//Set drawing state to the opposite of the first element clicked on
if( drawing_state_ == MazeElement::None )
{
if( maze_[ x * width_ + y].getState() == MazeElement::Wall)
{
drawing_state_ = MazeElement::Floor;
}
else if( maze_[ x * width_ + y].getState() == MazeElement::Floor)
{
drawing_state_ = MazeElement::Wall;
}
}
//draw the current state out to wherever the mouse is
maze_[ index( x, y, width_ ) ].setState( drawing_state_ );
}
}
else
{
//if the mouse is no longer being pressed, change the drawing state
drawing_state_ = MazeElement::None;
}
if( !auto_generate_ && !solving_ )
{
if( sf::Keyboard::isKeyPressed( sf::Keyboard::Q ) )
{
done_generating_ = true;
}
else if( sf::Keyboard::isKeyPressed( sf::Keyboard::A ) )
{
auto_generate_ = true;
first_generate_run_ = true;
done_generating_ = false;
}
else if( sf::Keyboard::isKeyPressed( sf::Keyboard::Num1 ) )
{
//place start block
//if the mouse button is being pressed, we want to draw
//so get the mouse position
sf::Vector2i mouse = sf::Mouse::getPosition( window );
//convert the mouse position to an x and y coordinate in our
//array of MazeElements
int x = mouse.x / MazeElement::getWidth();
int y = mouse.y / MazeElement::getHeight();
if( x < width_ && y < height_ &&
x >= 0 && y >= 0)
{
//draw the current state out to wherever the mouse is
maze_[ index( x, y, width_ ) ].setState( MazeElement::Floor );
maze_[ index( start_x_, start_y_, width_ ) ].resetColor();
start_x_ = x;
start_y_ = y;
maze_[ index( start_x_, start_y_, width_ ) ].setColor( start_block );
}
}
else if( sf::Keyboard::isKeyPressed( sf::Keyboard::Num2 ) )
{
//place stop block
//if the mouse button is being pressed, we want to draw
//so get the mouse position
sf::Vector2i mouse = sf::Mouse::getPosition( window );
//convert the mouse position to an x and y coordinate in our
//array of MazeElements
int x = mouse.x / MazeElement::getWidth();
int y = mouse.y / MazeElement::getHeight();
if( x < width_ && y < height_ &&
x >= 0 && y >= 0)
{
//draw the current state out to wherever the mouse is
maze_[ index( x, y, width_ ) ].setState( MazeElement::Floor );
maze_[ index( stop_x_, stop_y_, width_ ) ].resetColor();
stop_x_ = x;
stop_y_ = y;
maze_[ index( stop_x_, stop_y_, width_ ) ].setColor( stop_block );
}
}
}
}
else if( solving_ )
{
if( first_solving_run_ )
{
solver_ = AStar( maze_, height_, width_,
start_x_, start_y_,
stop_x_, stop_y_ );
first_solving_run_ = false;
}
solving_ = !solver_.update( maze_ );
}
if( sf::Keyboard::isKeyPressed( sf::Keyboard::C ) )
{
//clear
solving_ = false;
auto_generate_ = false;
done_generating_ = false;
first_generate_run_ = true;
first_solving_run_ = true;
for( int i = 0; i < height_ * width_; ++i)
{
maze_[i].setState( MazeElement::Floor );
}
}
else if( sf::Keyboard::isKeyPressed( sf::Keyboard::S ) )
{
//solve
solving_ = true;
auto_generate_ = false;
done_generating_ = true;
first_generate_run_ = true;
first_solving_run_ = true;
}
else if( sf::Keyboard::isKeyPressed( sf::Keyboard::E ) )
{
//edit
solving_ = false;
auto_generate_ = false;
done_generating_ = false;
first_generate_run_ = true;
first_solving_run_ = true;
for( int i = 0; i < height_ * width_; ++i)
{
maze_[i].resetColor();
}
maze_[ index( start_x_, start_y_, width_ ) ].setColor( start_block );
maze_[ index( stop_x_, stop_y_, width_ ) ].setColor( stop_block );
}
}
