00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039
00040 #include "getgraph.h"
00041
00042 uint WIDTH_PX;
00043 uint HEIGHT_PX;
00044 float RESOLUTION;
00045
00046
00047 float OFFSET_X;
00048 float OFFSET_Y;
00049
00050 uint GetGraphDimension (const char* graph_file){
00051
00052 FILE *file;
00053 file = fopen (graph_file,"r");
00054 uint dimension;
00055
00056 if(file == NULL){
00057 ROS_INFO("Can not open filename %s", graph_file);
00058 ROS_BREAK();
00059 }else{
00060 ROS_INFO("Graph File Opened. Reading Dimensions.\n");
00061 int r;
00062 r=fscanf (file, "%u", &dimension);
00063
00064
00065 r=fscanf (file, "%u", &WIDTH_PX);
00066 r=fscanf (file, "%u", &HEIGHT_PX);
00067 r=fscanf (file, "%f", &RESOLUTION);
00068 r=fscanf (file, "%f", &OFFSET_X);
00069 r=fscanf (file, "%f", &OFFSET_Y);
00070
00071
00072 }
00073 fclose(file);
00074 return dimension;
00075 }
00076
00077
00078 void GetGraphInfo (vertex *vertex_web, uint dimension, const char* graph_file){
00079
00080 FILE *file;
00081 file = fopen (graph_file,"r");
00082
00083 if(file == NULL){
00084 ROS_INFO("Can not open filename %s", graph_file);
00085 ROS_BREAK();
00086 }else{
00087 ROS_INFO("Graph File Opened. Getting Graph Info.\n");
00088
00089 uint i,j;
00090 float temp;
00091 int r;
00092
00093
00094 for (i=0; i<FIRST_VID-1; i++){
00095 r=fscanf (file, "%f", &temp);
00096 }
00097
00098 for (i=0;i<dimension;i++){
00099
00100 r=fscanf (file, "%u", &vertex_web[i].id);
00101
00102 r=fscanf (file, "%f", &vertex_web[i].x);
00103 vertex_web[i].x *= RESOLUTION;
00104 vertex_web[i].x += OFFSET_X;
00105
00106 r=fscanf (file, "%f", &vertex_web[i].y);
00107 vertex_web[i].y *= RESOLUTION;
00108 vertex_web[i].y += OFFSET_Y;
00109
00110 r=fscanf (file, "%u", &vertex_web[i].num_neigh);
00111
00112 for (j=0;j<vertex_web[i].num_neigh; j++){
00113 r=fscanf (file, "%u", &vertex_web[i].id_neigh[j]);
00114 r=fscanf (file, "%s", &vertex_web[i].dir[j]);
00115 r=fscanf (file, "%u", &vertex_web[i].cost[j]);
00116 }
00117
00118 }
00119
00120 }
00121
00122
00123
00124 fclose(file);
00125
00126 }
00127
00128 uint IdentifyVertex (vertex *vertex_web, uint size, double x, double y){
00129
00130 uint i, v=0;
00131 double dif_x, dif_y, result=INFINITY;
00132
00133 for (i=0; i<size; i++){
00134 dif_x = vertex_web[i].x - x;
00135 dif_y = vertex_web[i].y - y;
00136
00137
00138 if( result > fabs (dif_x) + fabs (dif_y) ){
00139 result = fabs (dif_x) + fabs (dif_y);
00140 v = i;
00141 }
00142 }
00143 return v;
00144 }
00145
00146 uint GetNumberEdges (vertex *vertex_web, uint dimension){
00147
00148 uint result = 0;
00149
00150 for (uint i=0; i<dimension; i++){
00151 for (uint j=0; j<vertex_web[i].num_neigh; j++){
00152 if (vertex_web[i].id < vertex_web[i].id_neigh[j]){
00153 result++;
00154 }
00155 }
00156 }
00157
00158 return result;
00159
00160 }
00161
00162
00163 char* itoa(int value, char* str, int radix) {
00164 static char dig[] =
00165 "0123456789"
00166 "abcdefghijklmnopqrstuvwxyz";
00167 int n = 0, neg = 0;
00168 unsigned int v;
00169 char* p, *q;
00170 char c;
00171
00172 if (radix == 10 && value < 0) {
00173 value = -value;
00174 neg = 1;
00175 }
00176 v = value;
00177 do {
00178 str[n++] = dig[v%radix];
00179 v /= radix;
00180 } while (v);
00181 if (neg)
00182 str[n++] = '-';
00183 str[n] = '\0';
00184
00185 for (p = str, q = p + (n-1); p < q; ++p, --q)
00186 c = *p, *p = *q, *q = c;
00187 return str;
00188 }
00189
00190