astar.cpp

一个非常有用的客户端开发程序

#include "astar.h"
#include "msg_types.h"
#include "com/ipc_com.h"
#include "util/timeutil.h"
#include "util/robotId.h"
#include<math.h>

#define DEBUG_ASTAR 0

AStar::AStar(int gridSize,double resolution)
{   _resolution=resolution;
   _gridSize=gridSize;
   _closedList=new PP_Point*[_gridSize];
   for(int i=0; i<_gridSize;i++){
      _closedList[i]=new PP_Point[_gridSize];
      for(int j=0; j<_gridSize;j++){
         _closedList[i][j].x=i;
         _closedList[i][j].y=j;
	_closedList[i][j].parent=0;
      }

   }
   _bumpList.clear();
}


AStar::~AStar()
{
   for(int i=0; i<_gridSize;i++)
      delete [] _closedList[i];
   delete [] _closedList;
}


list<PP_Point> AStar::findPlan(PP_Point start, PP_Point goal)
{ list <PP_Point> plan;
   plan.clear();
   //  rlogNote("starting from %d %d --> goal %d %d \n",_start.x,_start.y,_goal.x,_goal.y);

   if(!goal.valid)
      return plan;
   _start=start;
   _goal=goal;
   //rlogNote("starting from %d %d --> goal %d %d \n",_start.x,_start.y,_goal.x,_goal.y);

   //Reset closed list
   for(int i=0; i<_gridSize;i++) //FIXME 
      for(int j=0; j<_gridSize;j++){
         _closedList[i][j].closed=false;
         _closedList[i][j].parent=0;   
      }
   _priorityQueue.clear();


   plan.clear();
   PP_Point* current=&_closedList[_start.x][_start.y];

   //   rlogNote("starting from %d %d --> goal %d %d \n",_start.x,_start.y,_goal.x,_goal.y);
   _priorityQueue.add(heuristic(*current),&_closedList[current->x][current->y]);
   _closedList[current->x][current->y].closed=true;

   while(*current!=goal){
      //rlogNote("i\n");
      if(_priorityQueue.empty()){
         if (DEBUG_ASTAR) rlogWarning("No path found!\n");
         list<PP_Point> noplan;
         return noplan;
      }

      current=_priorityQueue.getNext();
      PP_Point* aux=current;
      int aux_x,aux_y;
      for(int i=-1;i<2;i++)
         for(int j=-1;j<2;j++){
            aux_x=current->x+i;
            aux_y=current->y+j;
            if(aux_x<_gridSize && aux_y<_gridSize && aux_x>=0 && aux_y>=0)
               if(!_closedList[aux_x][aux_y].closed &&_occupancy[aux_x][aux_y]<_threshold){
                  aux=&_closedList[aux_x][aux_y]; 
                  
                  double penalty=0;
                  PP_Point auxbump(aux_x,aux_y);
                  if(find(_bumpList.begin(),_bumpList.end(),auxbump)!=_bumpList.end()){
                     penalty=10000;
                  }

                  aux->cost=current->cost+(abs(i)+abs(j))*(1+10.0*(_occupancy[aux_x][aux_y]+penalty));
                  if(aux->parent!=0)
                     rlogError("BUGGGGGGGGGG\n");
                  aux->parent=current;
                  aux->closed=true;	
                  _priorityQueue.add(heuristic(*aux)+aux->cost,aux);

               }
         }
   }

   PP_Point* aux=current;
   while(*aux!=_start){ //get the plan
      //rlogNote("retriving plan %d %d\n",aux->x,aux->y);
      plan.push_front(*aux);        
      aux=aux->parent; 	
      if(aux==0){
         rlogError("Error extracting plan!\n");
         break;
         //throw "ERROR:THIS SHOULD NOT HAPPEN\n";//FIXME
      }
   } 
   //rlogNote("returning plan of size %d\n",plan.size());
   return plan;
}


PP_Point** AStar::getClosedList()
{
   return _closedList;
}


double AStar::heuristic(PP_Point node)
{
   return hypot(abs(node.x-_goal.x),abs(node.y-_goal.y));
   //return abs(node.x-_goal.x)+abs(node.y-_goal.y);
}




direction AStar::getDirection(PP_Point cur_loc, PP_Point next_loc){
   int x_dir=cur_loc.x -next_loc.x;
   int y_dir=cur_loc.y -next_loc.y;
   
   if(x_dir ==1 && y_dir==0)
      return e;
   else if(x_dir ==-1 && y_dir==0)
      return w;
   else if(x_dir ==0 && y_dir==-1)
      return n;
   else if(x_dir ==0 && y_dir==1)
      return s;
   else if(x_dir ==-1 && y_dir==1)
      return sw;
   else if(x_dir ==1 && y_dir==1)
      return se;
   else if(x_dir ==-1 && y_dir==-1)
      return nw;
   else if(x_dir ==1 && y_dir==-1)
      return ne;
   else {
      rlogError("ERROR IN WAY POINTS EXTRACTION!! (xdir,ydir)-->(%d, %d) \n",x_dir,y_dir);
       return err;
      }
}

list<PP_Point> AStar::getWayPoints(list<PP_Point> &path)
{   
   direction current_dir, old_dir;
   list<PP_Point> result;
   if(path.size() < 3 )
   {
      rlogWarning("Path too short\n");
      return result;
   }

   list<PP_Point>::iterator iter= path.begin();
   PP_Point curr_point=(*iter);
   iter++;
   result.push_back(curr_point);
   PP_Point next_point= (*iter);
   iter++;

   old_dir=getDirection(curr_point,next_point);
   curr_point=next_point;
   for(; iter != path.end(); iter++){
      next_point= (*iter);
      current_dir=getDirection(curr_point,next_point);	 
      if(current_dir!=old_dir && current_dir!=err)
         result.push_back(curr_point);
      curr_point=next_point;
      old_dir=current_dir;
   }
   result.push_back(*(--iter));
   //Smoothing
   if(ENABLE_WAYPOINT_SMOOTHING){
      list<PP_Point>::iterator wp_iter= result.begin();
      list<PP_Point>::iterator curr_wp=wp_iter;
      list<PP_Point> toDelete;
      list<PP_Point> result2;
      int count=0;
      PP_Point avg;
      for(; wp_iter != result.end(); wp_iter++){
         if (hypot(_gridSize/2 - (*wp_iter).x, _gridSize/2 - (*wp_iter).y) <= 2)
            continue;

         if(abs((*curr_wp).x-(*wp_iter).x)<3 && abs((*curr_wp).y-(*wp_iter).y)<3)
         {
            avg.x += (*wp_iter).x;
            avg.y += (*wp_iter).y;
            count++;

            if(count>3){
               avg.x /= count;
               avg.y /= count;
               result2.push_back(avg);
               count=0;
               avg.x = 0;
               avg.y = 0;
            }
         }
         else {
            if (count>0) {
               avg.x /= count;
               avg.y /= count;
               result2.push_back(avg);
               count=0;
               avg.x = 0;
               avg.y = 0;
            }
            avg.x += (*wp_iter).x;
            avg.y += (*wp_iter).y;
            count++;
         }

         curr_wp=wp_iter;
      }

      // Ensure to have goal in wp list
      list<PP_Point>::iterator last_wp = result2.end();
      if (!result2.empty()) {
         last_wp--; 
         wp_iter--;

         if ( *last_wp != *wp_iter ) 
            result2.push_back(*wp_iter);
      }
      else 
         result2.push_back(*(--wp_iter));
      return result2;
   }
   return result;
}



/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/