Revision cccc25c9
ID | cccc25c99e4cea26a0a2c76d7ab7121512d7b3d2 |
Made nav map into an actual workable behaviorgit statusgit status
scout/libscout/src/behaviors/navigationMap.cpp | ||
---|---|---|
1 | 1 |
#include "navigationMap.h" |
2 | 2 |
|
3 |
using namespace std; |
|
4 |
|
|
3 | 5 |
/** @brief Initializes the map */ |
4 |
navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname, "navigationMap")
|
|
6 |
navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigationMap") |
|
5 | 7 |
{ |
6 | 8 |
/** Initialize Map |
7 | 9 |
* |
... | ... | |
88 | 90 |
map.push_back(a12); |
89 | 91 |
|
90 | 92 |
curr_state = START_STATE; |
91 |
eta = 99999;
|
|
93 |
arrival_time = ros::TIME_MAX;
|
|
92 | 94 |
} |
93 | 95 |
|
94 | 96 |
/** @brief Goes through and frees all allocated memory */ |
... | ... | |
103 | 105 |
return; |
104 | 106 |
} |
105 | 107 |
|
106 |
navigationMap::run() |
|
108 |
void navigationMap::run()
|
|
107 | 109 |
{ |
108 |
|
|
110 |
Duration t; |
|
111 |
ROS_INFO("My state is: %d\n", curr_state); |
|
112 |
//Straight, straight, right, left, straight 5 |
|
113 |
update_state(ISTRAIGHT); |
|
114 |
ROS_INFO("My state is: %d\n", curr_state); |
|
115 |
t = get_time_remaining(); |
|
116 |
while(t.toSec() > 0) |
|
117 |
t = get_time_remaining(); |
|
118 |
update_state(ISTRAIGHT); |
|
119 |
ROS_INFO("My state is: %d\n", curr_state); |
|
120 |
t = get_time_remaining(); |
|
121 |
while(t.toSec() > 0) |
|
122 |
t = get_time_remaining(); |
|
123 |
update_state(IRIGHT); |
|
124 |
ROS_INFO("My state is: %d\n", curr_state); |
|
125 |
t = get_time_remaining(); |
|
126 |
while(t.toSec() > 0) |
|
127 |
t = get_time_remaining(); |
|
128 |
update_state(ILEFT); |
|
129 |
ROS_INFO("My state is: %d\n", curr_state); |
|
130 |
t = get_time_remaining(); |
|
131 |
while(t.toSec() > 0) |
|
132 |
t = get_time_remaining(); |
|
133 |
update_state(ISTRAIGHT); |
|
134 |
ROS_INFO("My state is: %d\n", curr_state); |
|
135 |
t = get_time_remaining(); |
|
136 |
while(t.toSec() > 0) |
|
137 |
t = get_time_remaining(); |
|
138 |
ROS_INFO("Traveled route!\n"); |
|
139 |
while(ok()) |
|
140 |
continue; |
|
109 | 141 |
} |
110 | 142 |
|
111 |
navigationMap::update_state(Turn turn_made) |
|
143 |
State navigationMap::update_state(Turn turn_made)
|
|
112 | 144 |
{ |
113 | 145 |
Edge* possible_edges = get_outbound_edges(curr_state); |
114 | 146 |
int arr_size = sizeof(possible_edges)/sizeof(Edge); |
... | ... | |
120 | 152 |
int speed = 10000;//its over 9000 |
121 | 153 |
curr_state = GET_EDGE_STATE(possible_edges[i]); |
122 | 154 |
//TODO: get actual speed |
123 |
arrival_time = ros::Time::now() +
|
|
124 |
GET_EDGE_DIST(possible_edges[i])/speed;
|
|
155 |
Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed);
|
|
156 |
arrival_time = Time::now() + travel_time;
|
|
125 | 157 |
return curr_state; |
126 | 158 |
} |
127 | 159 |
} |
128 | 160 |
return -1;//failure to succeed |
129 | 161 |
} |
130 | 162 |
|
131 |
navigationMap::get_eta() |
|
163 |
Time navigationMap::get_eta()
|
|
132 | 164 |
{ |
133 | 165 |
return arrival_time; |
134 | 166 |
} |
135 | 167 |
|
136 |
navigationMap::get_time_remaining() |
|
168 |
Duration navigationMap::get_time_remaining()
|
|
137 | 169 |
{ |
138 |
return (arrival_time - ros::Time::now());
|
|
170 |
return (arrival_time - Time::now()); |
|
139 | 171 |
} |
140 | 172 |
|
141 |
navigationMap::get_state() |
|
173 |
State navigationMap::get_state()
|
|
142 | 174 |
{ |
143 | 175 |
return curr_state; |
144 | 176 |
} |
145 | 177 |
|
146 |
navigationMap::get_outbound_edges(State state) |
|
178 |
Edge* navigationMap::get_outbound_edges(State state)
|
|
147 | 179 |
{ |
148 |
return map(state-1); |
|
180 |
return map.at(state-1);
|
|
149 | 181 |
} |
150 | 182 |
|
151 | 183 |
Path navigationMap::shortest_path(State target_state) |
Also available in: Unified diff