Revision b9e59a3c
Compiles.
scout/libscout/src/test_behaviors/smart_runaround.cpp | ||
---|---|---|
178 | 178 |
spinOnce(); |
179 | 179 |
|
180 | 180 |
// Assumption: readings are given in millimeters - Zane |
181 |
|
|
182 |
<<<<<<< HEAD |
|
183 |
// matrices for going from robot's frame to base frame |
|
184 |
float rightMat[2][2] = {{0, 1}, {-1, 0}}; |
|
185 |
float downMat[2][2] = {{-1, 0}, {0, -1}}; |
|
186 |
float leftMat[2][2] = {{0, 1}, {1, 0}}; |
|
187 |
|
|
188 |
// Look to the left (and update map). |
|
189 |
float left_distance = readings[0]; // w.r.t. robot |
|
190 |
if(left_distance == 0) |
|
191 |
return false; |
|
192 |
int left_idx = -mm_to_idx(left_distance); // w.r.t. map |
|
193 |
// plot to map if indices are valid |
|
194 |
if (0 <= left_idx && left_idx < MAP_LENGTH) |
|
195 |
map[row][col+left_idx] = SEEN; |
|
196 |
|
|
197 |
// Look in the other directions (and update map). |
|
198 |
for (int i = 24; i < 48; i++) { |
|
199 |
float distance = readings[i]; // w.r.t. robot |
|
200 |
if(distance == 0) |
|
201 |
return false; |
|
202 |
if(distance >= 500) |
|
203 |
break; // too far to be accurate |
|
204 |
float theta = (M_PI/24)*i - M_PI; |
|
205 |
float xDist = distance*cos(theta); // w.r.t. robot |
|
206 |
float yDist = distance*sin(theta); // w.r.t. robot |
|
207 |
float inputs[2] = {xDist, yDist}; |
|
208 |
float *ans; |
|
209 |
|
|
210 |
// re-orient x and y distances based on direction |
|
211 |
switch(dir) { |
|
212 |
case UP: |
|
213 |
ans[0] = xDist; |
|
214 |
ans[1] = yDist; |
|
215 |
break; |
|
216 |
case RIGHT: |
|
217 |
ans = matrix_mult(inputs, rightMat); |
|
218 |
break; |
|
219 |
case DOWN: |
|
220 |
ans = matrix_mult(inputs, downMat); |
|
221 |
break; |
|
222 |
case LEFT: |
|
223 |
ans = matrix_mult(inputs, leftMat); |
|
224 |
break; |
|
225 |
} |
|
226 |
// indices into the map |
|
227 |
int pixDistX = row + mm_to_idx(ans[0]); |
|
228 |
int pixDistY = col + mm_to_idx(ans[1]); |
|
229 |
// plot to map if indices are valid |
|
230 |
if (0 <= pixDistX && pixDistX < MAP_LENGTH |
|
231 |
&& 0 <= pixDistY && pixDistY < MAP_LENGTH) |
|
232 |
map[pixDistX][pixDistY] = SEEN; |
|
233 |
======= |
|
234 | 181 |
// distances with respect to robot, NOT map |
235 | 182 |
// Look to the left. |
236 | 183 |
float left_distance = readings[0]/1000.0; |
237 |
int left_idx = m_to_idx(left_distance); |
|
184 |
int left_idx = mm_to_idx(left_distance);
|
|
238 | 185 |
// Look to the front. |
239 | 186 |
float front_distance = readings[36]/1000.0; |
240 |
int front_idx = m_to_idx(front_distance); |
|
187 |
int front_idx = mm_to_idx(front_distance);
|
|
241 | 188 |
// Look to the right. |
242 | 189 |
float right_distance = readings[24]/1000.0; |
243 |
int right_idx = m_to_idx(right_distance); |
|
190 |
int right_idx = mm_to_idx(right_distance);
|
|
244 | 191 |
|
245 | 192 |
ROS_INFO("front: %lf left: %lf right: %lf", |
246 | 193 |
front_distance, left_distance, right_distance); |
... | ... | |
323 | 270 |
break; |
324 | 271 |
} |
325 | 272 |
map[row][col-l] = SEEN; |
326 |
>>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c |
|
327 | 273 |
} |
328 | 274 |
return true; |
329 | 275 |
} |
Also available in: Unified diff