Revision b9e59a3c
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 
// reorient 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][coll] = SEEN; 
326 
>>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c 

327  273 
} 
328  274 
return true; 
329  275 
} 
Also available in: Unified diff