getgraph.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2014, ISR University of Coimbra.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the ISR University of Coimbra nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: David Portugal (2011-2014), and Luca Iocchi (2014-2016)
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 //float WIDTH_M;
00046 //float HEIGHT_M;
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       //Initialize other dimension variables:
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       //WIDTH_M = (float) WIDTH_PX * RESOLUTION;
00071       //HEIGHT_M = (float) HEIGHT_PX * RESOLUTION;
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       //Start Reading the File from FIRST_VID On:
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; //convert to m
00104         vertex_web[i].x += OFFSET_X;
00105         
00106         r=fscanf (file, "%f", &vertex_web[i].y);
00107         vertex_web[i].y *= RESOLUTION; //convert to m
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]);        //can eventually be converted to meters also...
00116         }
00117         
00118       }     
00119       
00120    }
00121         
00122     //printf ("[v=10], x = %f (meters)\n",vertex_web[10].x); 
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 //     printf("[%u] result = %f, (dif_x+dif_y) = %f\n",i,result, fabs(dif_x) + fabs(dif_y));
00138     if( result > fabs (dif_x) + fabs (dif_y) ){ //Identify the Vertex closer to the initial coordinates x & 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 //integer to array (itoa for linux c)
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 


patrolling_sim
Author(s):
autogenerated on Mon Oct 2 2017 03:13:50