Revision c63c9752 scout/scoutsim/src/scout.cpp
scout/scoutsim/src/scout.cpp  

105  105 
linesensor_readings.push_back(0); 
106  106 
} 
107  107  
108 
meter = scout.GetHeight(); 

109  
110  108 
// Initialize sonar 
111  109 
sonar_position = 0; 
112  110 
sonar_stop_l = 0; 
...  ...  
140  138 
{ 
141  139 
motor_br_speed = msg>br_speed; 
142  140 
} 
143  
144  141 
} 
145  142  
146  143 
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, 
...  ...  
194  191 
} 
195  192  
196  193 
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, 
197 
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on) 

194 
double robot_theta, int sonar_pos, 

195 
wxMemoryDC& sonar_dc, bool sonar_on) 

198  196 
{ 
199  197 
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0)  PI / 2; 
200  198 
unsigned int d = 0; 
201  199  
202  200 
unsigned int reading = 0; 
203 
int d_x = 0; 

204 
int d_y = 0; 

201 
int d_x = 0; 

202 
int d_y = 0; 

203  
205  204 
do 
206  205 
{ 
207  206 
d_x = x + (int) floor(d * cos(angle)); 
...  ...  
233  232 
/// @todo Consider using different cutoffs for different features 
234  233 
while (reading < 128); /// @todo Get rid of hardcoded stuff like this 
235  234 

236 
if(sonar_on)


237 
{


238 
if(isFront)


239 
{


240 
// draw a circle at the wall_x, wall_y where reading > 128


241 
sonar_dc.SelectObject(*path_bitmap);


242 
sonar_dc.SetBrush(*wxRED_BRUSH); //old value


243 
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);


244 
old_front_dx = d_x;


245 
old_front_dy = d_y;


246 
}


247 
else


248 
{


249 
// draw a circle at the wall_x, wall_y where reading > 128


250 
sonar_dc.SelectObject(*path_bitmap);


251 
sonar_dc.SetBrush(*wxRED_BRUSH); //old value


252 
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);


253 
old_back_dx = d_x;


254 
old_back_dy = d_y;


255 
}


256  
257 
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value


258 
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);


259 
if (isFront) //for some reason isFront = (!isFront) is not working


260 
{


261 
isFront = FALSE;


262 
}


263 
else


264 
{


265 
isFront = TRUE;


266 
}


267 


268 
}


235 
if(sonar_on)


236 
{


237 
if(isFront)


238 
{


239 
// draw a circle at the wall_x, wall_y where reading > 128


240 
sonar_dc.SelectObject(*path_bitmap);


241 
sonar_dc.SetBrush(*wxRED_BRUSH); //old value


242 
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);


243 
old_front_dx = d_x;


244 
old_front_dy = d_y;


245 
}


246 
else


247 
{


248 
// draw a circle at the wall_x, wall_y where reading > 128


249 
sonar_dc.SelectObject(*path_bitmap);


250 
sonar_dc.SetBrush(*wxRED_BRUSH); //old value


251 
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);


252 
old_back_dx = d_x;


253 
old_back_dy = d_y;


254 
}


255  
256 
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value


257 
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);


258 
if (isFront) //for some reason isFront = (!isFront) is not working


259 
{


260 
isFront = FALSE;


261 
}


262 
else


263 
{


264 
isFront = TRUE;


265 
}


266 


267 
}


269  268 
return d; 
270  269 
} 
271  270  
...  ...  
333  332 
/// Sends back the position of this scout so scoutsim can save 
334  333 
/// the world state 
335  334 
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, 
336 
wxMemoryDC& sonar_dc,


337 
bool sonar_on,


335 
wxMemoryDC& sonar_dc,


336 
bool sonar_on,


338  337 
const wxImage& path_image, 
339  338 
const wxImage& lines_image, 
340  339 
const wxImage& walls_image, 
341  340 
wxColour background_color, 
342 
wxColour sonar_color,


341 
wxColour sonar_color,


343  342 
world_state state) 
344  343 
{ 
345  344 
// Assume that the two motors on the same side will be set to 
...  ...  
348  347 
float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2; 
349  348 
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; 
350  349  
351 
// Set the linear and angular speeds


352 
float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;


353 
float ang_vel = SPEED_SCALE_FACTOR * (r_speed  l_speed);


350 
// Find linear and angular movement in m


351 
float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;


352 
float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed  l_speed);


354  353  
355  354 
Vector2 old_pos = pos; 
356  355  
357  356 
// Update encoders 
358 
/// @todo replace 

359 
fl_ticks += (unsigned int) motor_fl_speed; 

360 
fr_ticks += (unsigned int) motor_fr_speed; 

361 
bl_ticks += (unsigned int) motor_bl_speed; 

362 
br_ticks += (unsigned int) motor_br_speed; 

357 
fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE * 

358 
ENCODER_TICKS_PER_METER); 

359 
fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE * 

360 
ENCODER_TICKS_PER_METER); 

361 
bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE * 

362 
ENCODER_TICKS_PER_METER); 

363 
br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE * 

364 
ENCODER_TICKS_PER_METER); 

365  
366 
orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI)); 

363  367  
364 
orient = fmod(orient + ang_vel * dt, 2*PI);


365 
pos.x += sin(orient + PI/2.0) * lin_vel * dt;


366 
pos.y += cos(orient + PI/2.0) * lin_vel * dt;


368 
// Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)


369 
pos.x += sin(orient + PI/2.0) * lin_dist;


370 
pos.y += cos(orient + PI/2.0) * lin_dist;


367  371  
368  372 
// Clamp to screen size 
369  373 
if (pos.x < 0  pos.x >= state.canvas_width 
370  374 
 pos.y < 0  pos.y >= state.canvas_height) 
371  375 
{ 
372 
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);


376 
ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y); 

373  377 
} 
374  378  
375  379 
pos.x = min(max(pos.x, 0.0f), state.canvas_width); 
376  380 
pos.y = min(max(pos.y, 0.0f), state.canvas_height); 
377  381  
378 
int canvas_x = pos.x * meter;


379 
int canvas_y = pos.y * meter;


382 
int canvas_x = pos.x * PIX_PER_METER;


383 
int canvas_y = pos.y * PIX_PER_METER;


380  384  
381  385 
{ 
382  386 
wxImage rotated_image = 
...  ...  
405  409 
p.x = pos.x; 
406  410 
p.y = state.canvas_height  pos.y; 
407  411 
p.theta = orient; 
408 
p.linear_velocity = lin_vel;


409 
p.angular_velocity = ang_vel;


412 
p.linear_velocity = l_speed;


413 
p.angular_velocity = r_speed;


410  414 
pose_pub.publish(p); 
411  415  
412  416 
update_linesensor(lines_image, canvas_x, canvas_y, p.theta); 
...  ...  
434  438 
{ 
435  439 
path_dc.SelectObject(*path_bitmap); 
436  440 
path_dc.SetPen(pen); 
437 
path_dc.DrawLine(pos.x * meter, pos.y * meter,


438 
old_pos.x * meter, old_pos.y * meter);


441 
path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,


442 
old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);


439  443 
} 
440  444 
} 
441  445  
...  ...  
450  454 
void Scout::paint(wxDC& dc) 
451  455 
{ 
452  456 
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); 
453 
dc.DrawBitmap(scout, pos.x * meter  (scout_size.GetWidth() / 2),


454 
pos.y * meter  (scout_size.GetHeight() / 2), true);


457 
dc.DrawBitmap(scout, pos.x * PIX_PER_METER  (scout_size.GetWidth() / 2),


458 
pos.y * PIX_PER_METER  (scout_size.GetHeight() / 2), true);


455  459 
} 
456  
457  460 
} 
Also available in: Unified diff