art_nav: art_nav: Blockage.cc Source File

00001 /*
00002  *  Base class for finite state machine events
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: Blockage.cc 435 2010-08-20 15:24:22Z jack.oquin $
00009  */
00010 
00011 #include "Blockage.h"
00012 
00013 void Blockages::pop_oldest()
00014 {
00015   double oldest_time;
00016   
00017   if (!empty())
00018     oldest_time=blocks.at(0).block_time;
00019   else return;
00020   
00021   uint index=0;
00022   for (uint i=1; i<blocks.size();i++)
00023     if (blocks.at(i).block_time < oldest_time)
00024       {
00025         oldest_time = blocks.at(i).block_time;
00026         index=i;
00027       }
00028   pop_block(index);
00029 }
00030 
00031 void Blockages::pop_newest()
00032 {
00033   double newest_time;
00034   
00035   if (!empty())
00036     newest_time=blocks.at(0).block_time;
00037   else return;
00038   
00039   uint index=0;
00040   for (uint i=1; i<blocks.size();i++)
00041     if (blocks.at(i).block_time > newest_time)
00042       {
00043         newest_time = blocks.at(i).block_time;
00044         index=i;
00045       }
00046   pop_block(index);
00047 }
00048 
00049 void Blockages::pop_block(uint index){
00050   
00051   block oblock=blocks.at(index);
00052   
00053   ROS_INFO_STREAM("Unblocking block #" << index);
00054 
00055   for (uint i=0; i< oblock.added_blocks.size(); i++)
00056     for (uint j=0; j< b_graph->edges_size; j++)
00057       if ((b_graph->edges.at(j).startnode_index ==
00058            oblock.added_blocks.at(i).startnode_index) &&
00059           (b_graph->edges.at(j).endnode_index ==
00060            oblock.added_blocks.at(i).endnode_index))
00061         {
00062           ROS_INFO_STREAM("Unblocking edge between "
00063                           <<b_graph->nodes[b_graph->edges.at(j).startnode_index].id.name().str
00064                           <<" and "
00065                           <<b_graph->nodes[b_graph->edges.at(j).endnode_index].id.name().str);
00066           b_graph->edges.at(j).blocked=false;
00067           break;
00068         }
00069 
00070   for (uint i=0; i< oblock.added_edges.size(); i++)
00071     for (uint j=0; j< b_graph->edges_size; j++)
00072       if ((b_graph->edges.at(j).startnode_index ==
00073            oblock.added_edges.at(i).startnode_index) &&
00074           (b_graph->edges.at(j).endnode_index ==
00075            oblock.added_edges.at(i).endnode_index))
00076         {
00077           ROS_INFO_STREAM("Deleting edge between "
00078                           <<b_graph->nodes[b_graph->edges.at(j).startnode_index].id.name().str
00079                           <<" and "
00080                           <<b_graph->nodes[b_graph->edges.at(j).endnode_index].id.name().str);
00081           b_graph->edges.erase(b_graph->edges.begin()+j);
00082           b_graph->edges_size--;
00083           break;
00084         }
00085   
00086   blocks.erase(blocks.begin()+index);
00087 }
00088 
00089 
00090 void Blockages::add_block(ElementID other_lane_way) 
00091 {
00092   block new_block;
00093   
00094   timeval time;
00095   gettimeofday(&time, NULL);
00096   
00097   new_block.block_time=time.tv_sec + (time.tv_usec/1000000.0);
00098   new_block.count=1;
00099   
00100   int other_lane_index=-1;
00101   int other_lane_other_side_index=-1;
00102   
00103   for (uint i=0; i< b_graph->nodes_size; i++)
00104     {
00105       if (b_graph->nodes[i].id == other_lane_way)
00106         other_lane_index=i;
00107       if ((b_graph->nodes[i].id.seg == other_lane_way.seg) &&
00108           (b_graph->nodes[i].id.lane == other_lane_way.lane) &&
00109           (b_graph->nodes[i].id.pt == other_lane_way.pt-1))
00110         other_lane_other_side_index=i;
00111     }
00112           
00113   if (other_lane_index == -1)
00114     return;
00115 
00116   WayPointEdge e1;
00117   
00118   if (!b_path->empty())
00119     e1=b_path->at(0);
00120   else return;
00121 
00122   bool exists_other_lane_connection=false;
00123 
00124   if (other_lane_other_side_index != -1)
00125     {
00126       for (uint i=0; i< b_graph->edges_size; i++)
00127         {
00128 
00129           if (b_graph->edges.at(i).endnode_index == other_lane_index)
00130             {
00131               b_graph->edges.at(i).blocked=true;
00132               new_block.added_blocks.push_back(b_graph->edges[i]);
00133               ROS_INFO_STREAM("Blocking edge between "
00134                               <<b_graph->nodes[b_graph->edges.at(i).startnode_index].id.name().str
00135                               <<" and "
00136                               <<b_graph->nodes[b_graph->edges.at(i).endnode_index].id.name().str);
00137               if (b_graph->edges.at(i).startnode_index == other_lane_other_side_index)
00138                 exists_other_lane_connection=true;
00139             }
00140           if ((b_graph->edges.at(i).endnode_index == e1.endnode_index) &&
00141               (b_graph->edges.at(i).startnode_index == e1.startnode_index))
00142             {
00143               ROS_INFO_STREAM("Blocking edge between "
00144                               <<b_graph->nodes[e1.startnode_index].id.name().str
00145                               <<" and "
00146                               <<b_graph->nodes[e1.endnode_index].id.name().str);
00147               b_graph->edges.at(i).blocked=true;
00148               new_block.added_blocks.push_back(b_graph->edges[i]);
00149             }
00150         }
00151     }
00152   
00153   WayPointEdge new_edge;
00154   new_edge.startnode_index=e1.startnode_index;
00155   new_edge.endnode_index=other_lane_index;
00156   new_edge.distance=100;
00157   new_edge.speed_max=DEFAULT_ZONE_SPEED;
00158   new_edge.blocked=false;
00159   
00160   ROS_INFO_STREAM("Adding edge between "
00161                   <<b_graph->nodes[new_edge.startnode_index].id.name().str
00162                   <<" and "
00163                   <<b_graph->nodes[new_edge.endnode_index].id.name().str);
00164 
00165   new_block.added_edges.push_back(new_edge);
00166   b_graph->edges.push_back(new_edge);
00167   b_graph->edges_size++;
00168 
00169   if (exists_other_lane_connection)
00170     {
00171       new_edge.startnode_index=other_lane_other_side_index;
00172       new_edge.endnode_index=e1.endnode_index;
00173       new_block.added_edges.push_back(new_edge);
00174       ROS_INFO_STREAM("Adding edge between "
00175                       <<b_graph->nodes[new_edge.startnode_index].id.name().str
00176                       <<" and "
00177                       <<b_graph->nodes[new_edge.endnode_index].id.name().str);
00178       b_graph->edges.push_back(new_edge);
00179       b_graph->edges_size++;
00180     }
00181   blocks.push_back(new_block);
00182 }