Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / navigationMap.cpp @ 7db6cf9f

History | View | Annotate | Download (10.7 KB)

1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

    
26
/**
27
 * @file navigationMap.cpp
28
 * @brief Contains navigation map Behavior declarations and definitions
29
 * 
30
 * Contains functions and definitions for the use of
31
 * navigation map Behavior 
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Priya Deo
35
 * @author Lalitha
36
 * @author James
37
 * @author Leon
38
 **/
39

    
40
#include "navigationMap.h"
41

    
42
using namespace std;
43

    
44
/**
45
 * @brief Initializes the navigation map
46
 *
47
 * Initialize the navigation map. 
48
 * The map itself is represented as a dynamic array of edge arrays
49
 * @param the string name of the scout
50
 */
51
navigationMap::navigationMap(string scoutname, Sensors* sensors) : 
52
            Behavior(scoutname, "navigationMap", sensors)
53
{
54
  /** Initialize Map 
55
   *                     _____
56
   *   1           2     |    | 3         4
57
   *  ----|-----------|--|----|--|---------|---------->
58
   *  <---|--5--------|--6-------|--7------|--8-------
59
   *      |           |          |         |
60
   *     9|         10|        11|       12|
61
   *      |           |          |         |
62
   *     ---13       ---14      ---15     ---16
63
   */
64
  
65
    Edge* a1 = new Edge[ARRAY_SIZE];
66
    a1[0] = MAKE_EDGE(ISTRAIGHT, 2, 10);
67
    a1[1] = MAKE_EDGE(IRIGHT, 13, 40);
68
    a1[2] = MAKE_EDGE(IUTURN, DEADEND, 0);
69

    
70
    Edge* a2 = new Edge[ARRAY_SIZE]; 
71
    a2[0] = MAKE_EDGE(ISTRAIGHT, 3, 10);
72
    a2[1] = MAKE_EDGE(IRIGHT, 14, 40);
73
    a2[2] = MAKE_EDGE(IUTURN, 5, 10);
74

    
75
    Edge* a3 = new Edge[ARRAY_SIZE]; 
76
    a3[0] = MAKE_EDGE(ISTRAIGHT, 4, 10);
77
    a3[1] = MAKE_EDGE(IRIGHT, 15, 40);
78
    a3[2] = MAKE_EDGE(IUTURN, 6, 10);
79

    
80
    Edge* a4 = new Edge[ARRAY_SIZE]; 
81
    a4[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);
82
    a4[1] = MAKE_EDGE(IRIGHT, 16, 40);
83
    a4[2] = MAKE_EDGE(IUTURN, 7, 10);
84

    
85
    Edge* a5 = new Edge[ARRAY_SIZE];
86
    a5[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);
87
    a5[1] = MAKE_EDGE(ILEFT, 13, 40);
88
    a5[2] = MAKE_EDGE(IUTURN, 2, 10);
89

    
90
    Edge* a6 = new Edge[ARRAY_SIZE];
91
    a6[0] = MAKE_EDGE(ISTRAIGHT, 5, 0);
92
    a6[1] = MAKE_EDGE(ILEFT, 14, 40);
93
    a6[2] = MAKE_EDGE(IUTURN, 3, 10);
94

    
95
    Edge* a7 = new Edge[ARRAY_SIZE];
96
    a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0);
97
    a7[1] = MAKE_EDGE(ILEFT, 15, 40);
98
    a7[2] = MAKE_EDGE(IUTURN, 4, 10);
99

    
100
    Edge* a8 = new Edge[ARRAY_SIZE];
101
    a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0);
102
    a8[1] = MAKE_EDGE(ILEFT, 16, 40);
103
    a8[2] = MAKE_EDGE(IUTURN, DEADEND, 10);
104

    
105
    Edge* a9 = new Edge[ARRAY_SIZE];
106
    a9[0] = MAKE_EDGE(IRIGHT, 2, 10);
107
    a9[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
108
    a9[2] = MAKE_EDGE(ISPOTTURN, 13, 40);
109

    
110
    Edge* a10 = new Edge[ARRAY_SIZE];
111
    a10[0] = MAKE_EDGE(IRIGHT, 3, 10);
112
    a10[1] = MAKE_EDGE(ILEFT, 5, 10);
113
    a10[2] = MAKE_EDGE(ISPOTTURN, 14, 40);
114

    
115
    Edge* a11 = new Edge[ARRAY_SIZE];
116
    a11[0] = MAKE_EDGE(IRIGHT, 4, 10);
117
    a11[1] = MAKE_EDGE(ILEFT, 6, 10);
118
    a11[2] = MAKE_EDGE(ISPOTTURN, 15, 40);
119

    
120
    Edge* a12 = new Edge[ARRAY_SIZE];
121
    a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
122
    a12[1] = MAKE_EDGE(ILEFT, 7, 10);
123
    a12[2] = MAKE_EDGE(ISPOTTURN, 16, 40);
124

    
125
    Edge* a13 = new Edge[ARRAY_SIZE];
126
    a13[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
127
    a13[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
128
    a13[2] = MAKE_EDGE(ISPOTTURN, 9, 40);
129

    
130
    Edge* a14 = new Edge[ARRAY_SIZE];
131
    a14[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
132
    a14[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
133
    a14[2] = MAKE_EDGE(ISPOTTURN, 10, 40);
134

    
135
    Edge* a15 = new Edge[ARRAY_SIZE];
136
    a15[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
137
    a15[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
138
    a15[2] = MAKE_EDGE(ISPOTTURN, 11, 40);
139

    
140
    Edge* a16 = new Edge[ARRAY_SIZE];
141
    a16[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
142
    a16[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
143
    a16[2] = MAKE_EDGE(ISPOTTURN, 12, 40);
144

    
145

    
146
    map.push_back(a1);
147
    map.push_back(a2);
148
    map.push_back(a3);
149
    map.push_back(a4);
150
    map.push_back(a5);
151
    map.push_back(a6);
152
    map.push_back(a7);
153
    map.push_back(a8);
154
    map.push_back(a9);
155
    map.push_back(a10);
156
    map.push_back(a11);
157
    map.push_back(a12);
158
    map.push_back(a13);
159
    map.push_back(a14);
160
    map.push_back(a15);
161
    map.push_back(a16);
162

    
163
    curr_state = START_STATE;
164
    arrival_time = ros::TIME_MAX;
165
}
166

    
167
/** @brief Goes through and frees all allocated memory */
168
navigationMap::~navigationMap()
169
{
170
  while(!map.empty())
171
  {
172
    Edge* temp = map.back();
173
    map.pop_back();
174
    delete temp;
175
  }
176
  return;
177
}
178

    
179
/** @brief FSM implementation*/
180
void navigationMap::run()
181
{
182
  Duration t;
183

    
184
  ROS_INFO("Going to state 16\n");
185
  Path path = shortest_path(16);
186
  if(path.path == NULL)
187
  {
188
    ROS_WARN("There is no path to state 16");
189
    return;
190
  }
191

    
192
  ROS_INFO("Worst case time to 16 is %d", get_worst_case_time(curr_state, 6).sec);
193

    
194
  for(int i=0; i<path.len && ok(); i++)
195
  {
196
    update_state(path.path[i]);
197
    ROS_INFO("Made turn %d, at state %d\n", path.path[i], curr_state);
198
    t = get_time_remaining();
199
    while(t.sec > 0)
200
      t = get_time_remaining();
201
    ROS_INFO("Now at state %d\n", curr_state);
202
  }
203

    
204
  ROS_INFO("Traveled route!\n");
205

    
206
  while(ok())
207
    continue;
208
}
209

    
210
/**@brief sets the current state to the state associated with the turn made
211
 * @param the Turn that we made from our state
212
 * @return our new State after making the turn 
213
 */
214
State navigationMap::update_state(Turn turn_made)
215
{
216
  Edge* possible_edges = get_outbound_edges(curr_state);
217
  for(int i=0;i<ARRAY_SIZE;i++)
218
  {
219
    //sets the current state to the state associated with the turn made
220
    if(GET_EDGE_DIR(possible_edges[i]) == turn_made)
221
    {
222
      //@todo: get actual speed
223
      int speed = 10;
224
      curr_state = GET_EDGE_STATE(possible_edges[i]);
225
      Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed);
226
      arrival_time = Time::now() + travel_time;
227
      return curr_state;
228
    }
229
  }
230
  return -1;//failure to succeed
231
}
232

    
233
/**@brief returns the predicted time of arrival for our current task
234
 * @return the predicted time of arrival for our current task
235
 */
236
Time navigationMap::get_eta()
237
{
238
  return arrival_time;
239
}
240

    
241
/**@brief returns the predicted amount of time it will take to finish our task
242
 * @return the predicted amount of time it will take to finish our task
243
 */
244
Duration navigationMap::get_time_remaining()
245
{
246
  return (arrival_time - Time::now());
247
}
248

    
249
/**@brief returns the current state of the scout in the map
250
 * @return the current State (ie: int) of the scout in the map
251
 */
252
State navigationMap::get_state()
253
{
254
  return curr_state;
255
}
256

    
257
/**@brief returns the Edges connecting from a given State
258
 * @param the State whose edges we want to get
259
 * @return the Edges connecting from the given State
260
 */
261
Edge* navigationMap::get_outbound_edges(State state)
262
{
263
  return map.at(state-1); 
264
}
265

    
266
/**@brief uses BFS to find the shortest path to a target State node
267
 * @param target_state the State that we want to get to
268
 * @return a Path struct containing the Turn* to take to get to the target state
269
 */
270
Path navigationMap::shortest_path(State target_state)
271
{
272
  return shortest_path(curr_state, target_state);
273
}
274

    
275
/**@brief uses BFS to find the shortest path to a target State node
276
 * @param start_state the State that we start from
277
 * @param target_state the State that we want to get to
278
 * @return a Path struct containing the Turn* to take to get to the target state
279
 */
280
Path navigationMap::shortest_path(State start_state, State target_state)
281
{
282
  // BFS algorithm
283
  State curr_state = start_state;
284
  int visited[MAX_NODES+1] = {0};
285

    
286
  queue<State> q;
287
  q.push(curr_state);
288
  // not zero = visited, zero = unvisited, negative = start state
289
  visited[curr_state] = -1;
290

    
291
  while (!q.empty())
292
  {
293
    State state = q.front();
294
    //actually dequeue it
295
    q.pop();
296
    if (state == target_state)
297
    {  
298
      Path path;
299
      path.path = (Turn*)calloc(sizeof(Turn), MAX_NODES);
300
      int j = 0; // counter
301
      for(State child = state; visited[child] >= 0; 
302
          child = visited[child]) //while not start state
303
      {
304
        State parent = visited[child];
305
        Edge* edges = get_outbound_edges(parent);
306
        for (int i = 0; i < ARRAY_SIZE; i++)
307
        {
308
          if (GET_EDGE_STATE(edges[i]) == child)
309
          {
310
            path.path[j] = GET_EDGE_DIR(edges[i]);
311
            j++;
312
            break;
313
          }
314
        }
315
      }
316
      /** Reverse moves list */
317
      for (int i = 0; i < j/2; i++)
318
      {
319
        path.path[i] ^= path.path[j-i-1];
320
        path.path[j-i-1] ^= path.path[i];
321
        path.path[i] ^= path.path[j-i-1];
322
      }
323
      path.len = j;
324
      return path;
325
    }
326
    Edge* edges = get_outbound_edges(state);
327

    
328
    for (int i = 0; i < ARRAY_SIZE; i++)
329
    {
330
      State new_state = GET_EDGE_STATE(edges[i]);
331
      if (new_state != DEADEND && !visited[new_state]) 
332
      {
333
        // set this state in visited as the parent of the new_state
334
        visited[new_state] = state;
335
        q.push(new_state);
336
      }
337
    }
338
  }
339
  //oops, no way to get to target from state
340
  Path path;
341
  path.len = 0;
342
  path.path = NULL;
343
  return path;
344
}
345

    
346
/** @brief returns worst case time to travel to dest
347
 *
348
 *  Takes into account speed of robot and interactions with other robots
349
 *
350
 *  @param start_state Node that we start at
351
 *  @param target_state Node that we end up at
352
 *  @return the worst case time to travel to target node
353
 */
354
Duration navigationMap::get_worst_case_time(State start_state, State target_state)
355
{
356
  Path path = shortest_path(start_state, target_state);
357
  Duration worst_case_time(0);
358

    
359
  State curr_state = start_state;
360
  //keep iterating over path while there are turns
361
  for(int i=0; i<path.len; i++)
362
  {
363
    Edge* edges = get_outbound_edges(curr_state); 
364
    for(int j=0; j<ARRAY_SIZE; j++)
365
    {
366
      if(GET_EDGE_DIR(edges[j]) == path.path[i])
367
      {
368
        Duration turn_time(TURN_TIME + (GET_EDGE_DIST(edges[j])/SPEED));
369
        worst_case_time += turn_time;
370
      }
371
    }
372
  }
373
  Duration additional_time(DROPOFF_TIME + WAIT_TIME);
374
  worst_case_time += additional_time; 
375

    
376
  return worst_case_time;
377
}