planner.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <math.h>
00004 
00005 #include "PathPlanning.h"
00006 #include "planner.h"
00007 #include "PathType.h"
00008 
00009 void path2pathseq(PathMapPtr a_path, IIS::TimedPath2DSeq *out_path);
00010 
00011 
00012 
00013 PathMapPtr make_path(void){
00014   PathMapPtr a_path;
00015 
00016   a_path = (PathMapPtr)malloc(sizeof(PathMap));
00017   if(!a_path)return 0;
00018 
00019   a_path->first_node = 0;
00020   a_path->last_node = 0;
00021   a_path->node_num = 0;
00022   a_path->link_num = 0;
00023 
00024   a_path->start_node = 0;
00025   a_path->goal_node = 0;
00026 
00027   return a_path;
00028 }
00029 
00030 void delete_path(PathMapPtr a_path){
00031   /*delete all path information*/
00032   int i,n;
00033   WayPointPtr a_node,next_node;
00034 
00035   printf("delete all path data\n");
00036   a_node = a_path->first_node;
00037   n = a_path->node_num;
00038   for(i = 0; i < n; i++){
00039     next_node = a_node->next;
00040     delete_node(a_path, a_node);
00041     a_node = next_node;
00042   }
00043   printf("%d %d\n", a_path->node_num, a_path->link_num);
00044   
00045   free((char*)a_path);
00046 
00047   printf("done.\n");
00048 }
00049 
00050 
00051 void print_path_info(PathMapPtr a_path){
00052   printf("----- path information -----\n");
00053   printf("node num \t:%d\n",a_path->node_num);
00054   printf("link num \t:%d\n",a_path->link_num);
00055 
00056   if(a_path->start_node){
00057     printf("start %f %f (%f %f)\n",a_path->start_x, a_path->start_y,
00058            a_path->start_node->x, a_path->start_node->y);
00059   }
00060   if(a_path->goal_node){
00061     printf("goal  %f %f (%f %f)\n",a_path->goal_x, a_path->goal_y,
00062            a_path->goal_node->x, a_path->goal_node->y);
00063   }
00064 }
00065 
00066 
00067 WayPointPtr make_node(PathMapPtr a_path, double x, double y){
00068   WayPointPtr a_node;
00069 
00070   a_node = (WayPointPtr)malloc(sizeof(WayPoint));
00071   a_node->x = x;
00072   a_node->y = y;
00073   a_node->link_num = 0;
00074   a_node->flag =0;
00075   a_node->cost = 10000000000.0;
00076   a_node->from = 0;
00077 
00078   if(!a_path->first_node){
00079     a_path->first_node = a_node;
00080     a_path->last_node = a_node;
00081   }else{
00082     a_path->last_node->next = a_node;
00083     a_path->last_node = a_node;
00084   }
00085   a_node->next =0;
00086 
00087   a_path->node_num++;
00088 
00089   return a_node;
00090 }
00091 
00092 int delete_node(PathMapPtr a_path, WayPointPtr a_node){
00093   int i;
00094 
00095   if(!a_node)return 0;
00096 
00097   /*delete all links*/
00098   for(i = 0; i < a_node->link_num; i++)
00099     delete_link(a_path,a_node->links[i]);
00100   a_path->node_num--;
00101 
00102   printf("delete node\n");
00103   free((char*)a_node);
00104   return 1;
00105 }
00106 
00107 
00108 WayPointPtr nth_node(PathMapPtr a_path,int n){
00109   WayPointPtr a_node;
00110   int i;
00111 
00112   a_node = a_path->first_node;
00113   
00114   for(i =0;i < n;i++){
00115     if(a_node->next)a_node= a_node->next;
00116     else{
00117       printf("ss\n");
00118       return 0; 
00119     
00120     }
00121   }
00122 
00123   return a_node;
00124 }
00125 
00126 LinkPtr make_link(PathMapPtr a_path, WayPointPtr node1, WayPointPtr node2, double cost){
00127   LinkPtr a_link;
00128 
00129   if(node1->link_num > INT_SEC_NUM-1|| 
00130      node2->link_num > INT_SEC_NUM-1 )return 0;/*cannot add link*/
00131 
00132   a_link = (LinkPtr)malloc(sizeof(Link));
00133   if(!a_link)return 0;
00134   
00135   /*regist node information*/
00136   a_link->node1 = node1;
00137   a_link->node2 = node2;
00138   a_link->cost = cost;
00139   a_link->color = 0;
00140   a_link->length =0;
00141   a_link->width = 0;
00142   a_link->maxspeed = 0;
00143 
00144   if(cost <= 0)a_link->cost = sqrt((node1->x-node2->x)*(node1->x-node2->x)+
00145                                    (node1->y-node2->y)*(node1->y-node2->y));
00146 
00147 
00148   /*regist new link pointer*/
00149   node1->links[node1->link_num++] = a_link;
00150   node2->links[node2->link_num++] = a_link;
00151 
00152   a_path->link_num++;
00153 
00154   return a_link;
00155 }
00156 
00157 int delete_link(PathMapPtr a_path, LinkPtr a_link){
00158   int i;
00159 
00160   if(!a_link)return -1;
00161 
00162   /*search linked pointer in table on node1*/
00163   for(i = 0;i < a_link->node1->link_num;i++)
00164     if(a_link->node1->links[i]==a_link)break;
00165 
00166   /*compress table*/
00167   if(i !=  a_link->node1->link_num){
00168     //printf("find \n");
00169     for(;i < a_link->node1->link_num-1;i++)
00170       a_link->node1->links[i] = a_link->node1->links[i+1];
00171     a_link->node1->link_num--;
00172   }else{
00173      printf("can not find \n");
00174   }
00175 
00176   /*search linked pointer in table on node2*/
00177   for(i = 0;i < a_link->node2->link_num;i++)
00178     if(a_link->node2->links[i]==a_link)break;
00179 
00180   /*compress table*/
00181   if(i !=  a_link->node2->link_num){
00182     //   printf("find \n");
00183     for(;i < a_link->node2->link_num-1;i++)
00184       a_link->node2->links[i] = a_link->node2->links[i+1];
00185     a_link->node2->link_num--;
00186   }else{
00187     printf("can not find \n");
00188   }
00189 
00190   a_path->link_num--;
00191   printf("delete link\n");
00192   free((char*)a_link);
00193  
00194   return 1;
00195 }
00196 
00197 
00198 
00199 int load_path_file(const char *file_name, PathMapPtr a_path){
00200   FILE *path_file;
00201 
00202   char kind;
00203   double x,y,theta,cost;
00204   WayPointPtr a_node;
00205   int n1,n2;
00206   int len;
00207   path_file = fopen(file_name,"r");
00208   if(!path_file){
00209     fprintf(stderr,"cannot open the assigned file.\n");
00210     return 0;
00211   }
00212 
00213 
00214   while(fscanf(path_file,"%c",&kind) != EOF){
00215     switch(kind){
00216     case 'n':
00217       len = fscanf(path_file,"%lf %lf %lf", &x, &y,&theta);    
00218       if(!(a_node = make_node(a_path, x, y)))
00219         printf("make_node error\n");
00220       printf("node (%f, %f)\n",x,y);
00221       break;
00222     case 'l':
00223       len = fscanf(path_file,"%d %d %lf", &n1,&n2,&cost);    
00224       if(!make_link(a_path, nth_node(a_path,n1), nth_node(a_path,n2), cost))
00225         printf("make_link error\n");
00226       printf("link %d-%d cost %f\n",n1,n2,cost);
00227       break;
00228     default:
00229       break;
00230     }
00231   } 
00232   return 1;
00233 }
00234 
00235 /*search nearest node (brute force search)*/
00236 WayPointPtr search_nearest_node(PathMapPtr a_path, double x, double y){
00237   int i;
00238   WayPointPtr a_node,nearest_node;
00239   double d,min_d;
00240 
00241   a_node = a_path->first_node;
00242   min_d = 10000;
00243   printf("input %f %f\n",x,y);
00244   for(i = 0;i < a_path->node_num;i++){
00245     if(!a_node)return 0; 
00246    d = (x -a_node->x)*(x-a_node->x) + (y-a_node->y)*(y-a_node->y);
00247     if(min_d > d){
00248       min_d = d;
00249       nearest_node = a_node;
00250     }
00251     a_node = a_node->next;
00252  
00253   }
00254   printf("near  %f %f\n",nearest_node->x,nearest_node->y);
00255   return nearest_node;
00256 }
00257 
00258 
00259 
00260 LinkPtr search_nearest_link(PathMapPtr a_path, double x, double y){
00261   LinkPtr nearest_link=0;
00262 
00263   return nearest_link;
00264 
00265 }
00266 
00267 /*input current position*/
00268 void set_start_position(PathMapPtr a_path, double x, double y){
00269 
00270   a_path->start_x = x;
00271   a_path->start_y = y;
00272   a_path->start_node = search_nearest_node(a_path,x,y);
00273   a_path->start_node->cost = 0;
00274 }
00275 
00276 /*input target  position*/
00277 void set_goal_position(PathMapPtr a_path, double x, double y){
00278   a_path->goal_x = x;
00279   a_path->goal_y = y;
00280   a_path->goal_node = search_nearest_node(a_path,x,y);
00281 
00282 }
00283 
00284 
00285 WayPointPtr next_queue(QueueListHandle a_queue){
00286   QueueListPtr next_queue;
00287   WayPointPtr a_node;
00288   
00289     if(!*a_queue)return 0;
00290 
00291   a_node = (*a_queue)->node;
00292 
00293   next_queue= (*a_queue)->next;
00294   free((char*)(*a_queue));
00295   (*a_queue) = next_queue;
00296 
00297   return a_node;
00298 }
00299 
00300 
00301 /*a_queueの後ろにキューをつくる*/
00302 QueueListPtr add_queue(QueueListHandle first_queue, WayPointPtr a_node){
00303   QueueListPtr new_queue;
00304   QueueListPtr a_queue;
00305 
00306   a_queue = *first_queue;
00307 
00308   new_queue = (QueueListPtr)malloc(sizeof(QueueList));
00309 
00310   if(!a_queue){
00311     printf("add new\n");  
00312     *first_queue = new_queue;
00313     new_queue->next = 0;
00314     new_queue->node = a_node;
00315   }else if(a_queue->node->cost < a_node->cost){
00316     printf("add first\n");
00317     *first_queue = new_queue;
00318     new_queue->next = a_queue;
00319     new_queue->node = a_node;
00320   }else{
00321     while(a_queue->next){
00322       if(a_queue->next->node->cost > a_node->cost){
00323         printf("add \n");
00324         new_queue->next = a_queue->next;
00325         a_queue->next   = new_queue;
00326         new_queue->node = a_node;
00327         break;
00328       }
00329       a_queue = a_queue->next;
00330     }
00331   }
00332 
00333   return new_queue;
00334 }
00335 
00336 
00337 
00338 /*solve*/
00339 PathMapPtr solve_minimum_path(PathMapPtr a_path){
00340   QueueListPtr queue_list;
00341   int i,goal_reached;
00342   double min_cost;
00343   WayPointPtr a_node,new_node,bef_node;
00344   PathMapPtr min_path;
00345  
00346   min_cost = 1e100; 
00347   queue_list = 0;
00348   add_queue(&queue_list, a_path->start_node);
00349   printf("start %f %f\n",a_path->start_node->x,a_path->start_node->y);
00350   printf("goal  %f %f\n",a_path->goal_node->x,a_path->goal_node->y);
00351 
00352   min_path = make_path();
00353   goal_reached = 0;
00354   while((a_node=next_queue(&queue_list)) != 0 && !goal_reached){/*キューから一つ読み出す*/
00355 
00356     if(a_node == a_path->goal_node){
00357       printf("goal\n");
00358       if(min_cost > a_node->cost)min_cost = a_node->cost;
00359     }
00360 
00361     if(a_node->cost > min_cost)continue;
00362 
00363     printf("%f %f %f \n",a_node->x, a_node->y,a_node->cost);
00364     /*ノードをキューに登録する*/
00365     for(i = 0;i < a_node->link_num; i++){  
00366       if(a_node->links[i]->node1 == a_node){
00367         
00368         if(a_node->cost + a_node->links[i]->cost <
00369            a_node->links[i]->node2->cost) {
00370           add_queue(&queue_list, a_node->links[i]->node2);        
00371           a_node->links[i]->node2->from = a_node;
00372           a_node->links[i]->color = 1;
00373           a_node->links[i]->node2->cost = 
00374             a_node->cost + a_node->links[i]->cost;
00375 
00376         }
00377       }else{
00378         if(a_node->cost + a_node->links[i]->cost <
00379            a_node->links[i]->node1->cost){
00380           add_queue(&queue_list, a_node->links[i]->node1);
00381           a_node->links[i]->color = 1;
00382           a_node->links[i]->node1->from = a_node;
00383           a_node->links[i]->node1->cost = 
00384             a_node->cost + a_node->links[i]->cost;
00385           
00386           //      if(a_node->cost > min_cost){goal_reached = 1;break;}
00387         }
00388       }
00389     }
00390   }
00391 
00392   /*back trace*/
00393   a_node = a_path->goal_node;
00394   bef_node = make_node(min_path, a_node->x, a_node->y);
00395   printf("%f %f\n",a_node->x,a_node->y );
00396   a_node = a_node->from;
00397   while(a_node){
00398     printf("%f %f\n",a_node->x,a_node->y );
00399     new_node = make_node(min_path,a_node->x,a_node->y);
00400     make_link(min_path,new_node,bef_node,1);
00401     a_node = a_node->from;
00402 
00403     bef_node =new_node;
00404 
00405   }
00406 
00407   //clear node cost
00408   a_node = a_path->first_node;
00409   for(i = 0;i < a_path->node_num;i++){
00410     if(!a_node)return 0; 
00411     a_node ->cost = 1e100;
00412     a_node->flag =0;
00413     a_node->from = 0;
00414     a_node = a_node->next;
00415   }
00416 
00417 
00418 
00419   return min_path;
00420 }
00421 
00422 void write_path(FILE *a_file, PathMapPtr a_path){
00423   WayPointPtr a_node;
00424   int i;
00425 
00426   a_node = a_path->first_node;
00427   while(a_node){
00428     for(i =0;i <  a_node->link_num;i++){
00429       fprintf(a_file, "%f %f \n%f %f\n\n",
00430               a_node->links[i]->node1->x,a_node->links[i]->node1->y,
00431               a_node->links[i]->node2->x,a_node->links[i]->node2->y);
00432     }
00433     a_node = a_node->next;
00434   }
00435 
00436 
00437 }
00438 
00439 void path2pathseq(PathMapPtr a_path, IIS::TimedPath2DSeq *out_path){
00440   WayPointPtr a_node;
00441   int i,num;
00442 
00443   a_node = a_path->first_node;
00444     if(!a_node)return;
00445   
00446   i=1;
00447   while(a_node->next){
00448     i++;
00449     a_node = a_node->next;
00450   }
00451   num = i;
00452   //  if(i<=1)return;
00453 
00454   //  out_path->path_list.length(i);
00455   out_path->id.length(i);
00456   out_path->pose.length(i);
00457   out_path->velocity.length(i);
00458   out_path->error.length(i);
00459 
00460   // printf("1 %d\n",i);  
00461   
00462   a_node = a_path->first_node;
00463   /*//i = 0;
00464   out_path->path_list[i].type = RUN_LINEFOLLOW;
00465   out_path->path_list[i].v = 0.3;
00466   out_path->path_list[i].x = a_node->x;
00467   out_path->path_list[i].y = a_node->y;
00468   out_path->path_list[i].theta =
00469     atan2(a_node->y-a_node->next->y,
00470           a_node->x-a_node->next->x );
00471   */
00472   /*旧
00473     out_path->path_list[i].type = RUN_LINEFOLLOW;
00474     out_path->path_list[i].v = 0.3;
00475     out_path->path_list[i].x = a_node->x;
00476     out_path->path_list[i].y = a_node->y;
00477   */
00478    
00479   while(a_node->next){ printf("%d\n",i);
00480     i--;
00481     out_path->id[i] = i;
00482     out_path->velocity[i].vx = 1.0;
00483     out_path->velocity[i].vy = 0.0;
00484     out_path->velocity[i].va = 0.0;
00485     out_path->pose[i].position.x = a_node->x;
00486     out_path->pose[i].position.y = a_node->y;
00487     out_path->error[i] = 0.0;
00488     out_path->pose[i].heading =
00489       atan2(a_node->y-a_node->next->y,
00490             a_node->x-a_node->next->x );
00491     
00492     a_node = a_node->next;
00493     //i++;
00494   }
00495 
00496   i--;
00497   out_path->id[0] = 0;
00498   out_path->velocity[0].vx = 1.0;
00499   out_path->velocity[0].vy = 0.0;
00500   out_path->velocity[0].va = 0.0;
00501   out_path->pose[0].position.x = a_node->x;
00502   out_path->pose[0].position.y = a_node->y;
00503   out_path->pose[0].heading = 0;
00504   out_path->error[0] = 0.0;
00505 
00506 
00507   //  printf("2\n");  
00508   //out_path->path_list[0].theta = 0;//out_path->path_list[i-1].theta ;
00509   //    a_node->y-a_node->links[0]->node2->y,
00510   //  a_node->x-a_node->links[0]->node2->x);
00511 
00512   //  printf("3\n");  
00513 }
00514 
00515 void draw_path(PathMapPtr a_path,PathMapPtr min_path){
00516   static FILE *gnuplot;
00517   FILE *path_file, *path_file2;
00518   //  WayPointPtr a_node;
00519  
00520   if(!gnuplot){
00521     gnuplot =popen("/usr/bin/gnuplot","w");
00522     fprintf(gnuplot,"set term x11\n");
00523   }
00524   path_file = fopen("path","w");
00525   path_file2 = fopen("path2","w");
00526 
00527   write_path(path_file,a_path);
00528   write_path(path_file2,min_path);
00529   fclose(path_file);
00530   fclose(path_file2);
00531 
00532   fprintf(gnuplot,"set xrange [-16:16]\n");
00533   fprintf(gnuplot,"set yrange [-12:12]\n");
00534   fprintf(gnuplot,"plot 'path' w l, 'path2' w l\n");
00535   //  fprintf(gnuplot,"plot 'path' w l\n");
00536   fflush(gnuplot);
00537   //  getchar();
00538   //pclose(gnuplot);
00539 
00540 }
00541 
00542 #if 0
00543 
00544 int main(int argc, char* argv[]){
00545   PathMapPtr minimum_path;
00546   PathMapPtr all_path;
00547   
00548   /*load path file*/
00549   all_path = make_path();
00550   load_path_file(argv[1], all_path);
00551 
00552   /*input current position*/
00553   set_start_position(all_path, 5, 3);
00554 
00555   /*input target  position*/
00556   set_goal_position(all_path, 1, 1);
00557   
00558   /*print created path information*/
00559   print_path_info(all_path);
00560 
00561   /*solve*/
00562   minimum_path=solve_minimum_path(all_path);
00563   draw_path(all_path,minimum_path);
00564   
00565 
00566   //  delete_path(all_path);
00567 }
00568 #endif 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


RS003
Author(s):
autogenerated on Thu Jun 27 2013 14:58:49