root / scout / libscout / src / behaviors / navigationMap.h @ 16d4e150
History | View | Annotate | Download (4.54 KB)
1 | 2d697b1f | Leon | /**
|
---|---|---|---|
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.h
|
||
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 | d8caf546 | Priya | #ifndef _NAVIGATION_MAP_
|
41 | #define _NAVIGATION_MAP_
|
||
42 | |||
43 | #include <cstdlib> |
||
44 | aa5e4ddc | Leon | #include <queue> |
45 | cccc25c9 | Priya | #include "../Behavior.h" |
46 | //#include "lineDrive.h" // Get turn Macros
|
||
47 | |||
48 | 2d697b1f | Leon | /** Turn defintions */
|
49 | cccc25c9 | Priya | #define ISTRAIGHT 0 |
50 | 58c19c15 | Priya | #define ILEFT 1 |
51 | cccc25c9 | Priya | #define IRIGHT 2 |
52 | #define IUTURN 3 |
||
53 | 58c19c15 | Priya | #define ISPOTTURN 4 |
54 | d8caf546 | Priya | |
55 | 1905324e | Priya | #define START_STATE 3 |
56 | d8caf546 | Priya | |
57 | 6642eee3 | Priya | #define DEADEND 255 |
58 | d8caf546 | Priya | #define ARRAY_SIZE 3 |
59 | 58c19c15 | Priya | #define MAX_NODES 16 |
60 | cc399ab3 | Priya | |
61 | #define SPEED 10 //distance_unit per s |
||
62 | #define DROPOFF_TIME 10 //seconds |
||
63 | #define TURN_TIME 3 //Seconds |
||
64 | #define NUM_ROBOTS 2 |
||
65 | #define WAIT_TIME (TURN_TIME * NUM_ROBOTS) //Seconds |
||
66 | 2d697b1f | Leon | |
67 | /** used to extract information from an encoded Edge */
|
||
68 | 58c19c15 | Priya | #define GET_EDGE_DIR(edge) ((edge)&0xF) |
69 | #define GET_EDGE_STATE(edge) (((edge)>>4)&0xFF) |
||
70 | #define GET_EDGE_DIST(edge) (((edge)>>12)&0xFFFFF) |
||
71 | d8caf546 | Priya | |
72 | 2d697b1f | Leon | /** used to change or build an Edge's information */
|
73 | 58c19c15 | Priya | #define SET_EDGE_DIR(dir) ((dir)&0xF) |
74 | #define SET_EDGE_STATE(state) (((state)&0xFF)<<4) |
||
75 | #define SET_EDGE_DIST(dist) (((dist)&0xFFFFF)<<12) |
||
76 | d8caf546 | Priya | |
77 | #define MAKE_EDGE(dir, state, dist) \
|
||
78 | 738e44fb | Priya | SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist) |
79 | |||
80 | 2d697b1f | Leon | /** an integer with a direction, an associated state, and distance
|
81 | * encoded into its bits*/
|
||
82 | cc399ab3 | Priya | typedef unsigned int Edge; |
83 | 2d697b1f | Leon | |
84 | /** a simple number representing the number of a node*/
|
||
85 | 6642eee3 | Priya | typedef unsigned int State; |
86 | 2d697b1f | Leon | |
87 | /** a number representing a type of turn, as defined above*/
|
||
88 | cc399ab3 | Priya | typedef unsigned int Turn; |
89 | d8caf546 | Priya | |
90 | 2d697b1f | Leon | /** a list of turns to follow a path */
|
91 | 738e44fb | Priya | typedef struct{ |
92 | int len;
|
||
93 | Turn* path; |
||
94 | } Path; |
||
95 | |||
96 | d8caf546 | Priya | class navigationMap : Behavior |
97 | { |
||
98 | d37cfc69 | Leon | /** ASCII Representation of the map
|
99 | *
|
||
100 | * 1 2 3 4
|
||
101 | * ----|-----------|----------|---------|---------->
|
||
102 | * <---|--5--------|--6-------|--7------|--8-------
|
||
103 | * | | | |
|
||
104 | * 9| 10| 11| 12|
|
||
105 | * | | | |
|
||
106 | 58c19c15 | Priya | * ---13 ---14 ---15 ---16
|
107 | d37cfc69 | Leon | */
|
108 | |||
109 | 738e44fb | Priya | public:
|
110 | 2d697b1f | Leon | /** Initializes the navigation map */
|
111 | d140fd71 | Yuyang | navigationMap(std::string scoutname, Sensors* sensors); |
112 | 2d697b1f | Leon | /** Goes through and frees all allocated memory */
|
113 | 738e44fb | Priya | ~navigationMap(); |
114 | |||
115 | 2d697b1f | Leon | /** FSM implementation */
|
116 | 738e44fb | Priya | void run();
|
117 | 2d697b1f | Leon | |
118 | /** sets the current state to the state associated with the turn made */
|
||
119 | 738e44fb | Priya | State update_state(Turn turn_made); |
120 | |||
121 | 2d697b1f | Leon | /** returns the predicted time of arrival for our current task */
|
122 | 738e44fb | Priya | Time get_eta(); |
123 | 2d697b1f | Leon | /** returns the predicted amount of time it will take to finish our task */
|
124 | cccc25c9 | Priya | Duration get_time_remaining(); |
125 | 738e44fb | Priya | |
126 | 2d697b1f | Leon | /** returns the Edges connecting from a given State */
|
127 | Edge* get_outbound_edges(State state); |
||
128 | |||
129 | /** returns the current state of the scout in the map*/
|
||
130 | 738e44fb | Priya | State get_state(); |
131 | 2d697b1f | Leon | /** uses BFS to find the shortest path to a target State node */
|
132 | cc399ab3 | Priya | Path shortest_path(State start_state, State target_state); |
133 | 738e44fb | Priya | Path shortest_path(State target_state); |
134 | |||
135 | cc399ab3 | Priya | /** returns the predicted worst case time it will take to travel from src to dest nodes */
|
136 | Duration get_worst_case_time(State start_state, State target_state); |
||
137 | |||
138 | 738e44fb | Priya | private:
|
139 | 2d697b1f | Leon | /** the dynamic array of edge arrays representing individual State nodes */
|
140 | cccc25c9 | Priya | std::vector <Edge*> map; |
141 | 2d697b1f | Leon | /** the current State node */
|
142 | 738e44fb | Priya | State curr_state; |
143 | 2d697b1f | Leon | /** the predicted time of arrival for our current task */
|
144 | 738e44fb | Priya | Time arrival_time; |
145 | d8caf546 | Priya | }; |
146 | #endif |