Revision 0e77683c
ID | 0e77683c65f392af332f84aa9b31eb8614290a55 |
Added built-in scoutsim teleop!
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
135 | 135 |
wireless_send = nh.subscribe("/wireless/send", 1000, |
136 | 136 |
&SimFrame::wirelessCallback, this); |
137 | 137 |
|
138 |
// Teleop |
|
139 |
teleop_type = TELEOP_OFF; |
|
140 |
teleop_l_speed = 0; |
|
141 |
teleop_r_speed = 0; |
|
142 |
teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000); |
|
143 |
|
|
138 | 144 |
ROS_INFO("Starting scoutsim with node name %s", |
139 | 145 |
ros::this_node::getName().c_str()) ; |
140 | 146 |
|
... | ... | |
151 | 157 |
menuView->Append(ID_LINES, _("&Lines")); |
152 | 158 |
menuView->Append(ID_WALLS, _("&Walls")); |
153 | 159 |
|
160 |
wxMenu *menuTeleop = new wxMenu; |
|
161 |
menuTeleop->Append(ID_TELEOP_NONE, _("&None")); |
|
162 |
menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise")); |
|
163 |
menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid")); |
|
164 |
|
|
154 | 165 |
wxMenuBar *menuBar = new wxMenuBar; |
155 | 166 |
menuBar->Append(menuFile, _("&File")); |
156 | 167 |
menuBar->Append(menuSim, _("&Sim")); |
157 | 168 |
menuBar->Append(menuView, _("&View")); |
169 |
menuBar->Append(menuTeleop, _("&Teleop")); |
|
158 | 170 |
|
159 | 171 |
SetMenuBar(menuBar); |
160 | 172 |
|
... | ... | |
293 | 305 |
{ |
294 | 306 |
ros::spinOnce(); |
295 | 307 |
|
308 |
teleop(); |
|
309 |
|
|
296 | 310 |
updateScouts(); |
297 | 311 |
|
298 | 312 |
if (!ros::ok()) |
... | ... | |
315 | 329 |
} |
316 | 330 |
} |
317 | 331 |
|
332 |
void SimFrame::stopTeleop(wxCommandEvent& event) |
|
333 |
{ |
|
334 |
teleop_type = TELEOP_OFF; |
|
335 |
teleop_l_speed = 0; |
|
336 |
teleop_r_speed = 0; |
|
337 |
} |
|
338 |
|
|
339 |
void SimFrame::startTeleopPrecise(wxCommandEvent& event) |
|
340 |
{ |
|
341 |
teleop_type = TELEOP_PRECISE; |
|
342 |
teleop_l_speed = 0; |
|
343 |
teleop_r_speed = 0; |
|
344 |
} |
|
345 |
|
|
346 |
void SimFrame::startTeleopFluid(wxCommandEvent& event) |
|
347 |
{ |
|
348 |
teleop_type = TELEOP_FLUID; |
|
349 |
teleop_l_speed = 0; |
|
350 |
teleop_r_speed = 0; |
|
351 |
teleop_fluid_speed = 0; |
|
352 |
teleop_fluid_omega = 0; |
|
353 |
} |
|
354 |
|
|
355 |
void SimFrame::teleop_move_precise() |
|
356 |
{ |
|
357 |
// Default to stop |
|
358 |
teleop_l_speed = 0; |
|
359 |
teleop_r_speed = 0; |
|
360 |
|
|
361 |
if (wxGetKeyState(WXK_UP)) |
|
362 |
{ |
|
363 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
364 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
365 |
} |
|
366 |
else if (wxGetKeyState(WXK_DOWN)) |
|
367 |
{ |
|
368 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
369 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
370 |
} |
|
371 |
else if (wxGetKeyState(WXK_LEFT)) |
|
372 |
{ |
|
373 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
374 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
375 |
} |
|
376 |
else if (wxGetKeyState(WXK_RIGHT)) |
|
377 |
{ |
|
378 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
379 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
380 |
} |
|
381 |
} |
|
382 |
|
|
383 |
void SimFrame::teleop_move_fluid() |
|
384 |
{ |
|
385 |
if (wxGetKeyState(WXK_UP)) |
|
386 |
{ |
|
387 |
teleop_fluid_speed += 2; |
|
388 |
} |
|
389 |
else if (wxGetKeyState(WXK_DOWN)) |
|
390 |
{ |
|
391 |
teleop_fluid_speed -= 2; |
|
392 |
} |
|
393 |
else if (teleop_fluid_speed > 0) |
|
394 |
{ |
|
395 |
teleop_fluid_speed -= 1; |
|
396 |
} |
|
397 |
else if (teleop_fluid_speed < 0) |
|
398 |
{ |
|
399 |
teleop_fluid_speed += 1; |
|
400 |
} |
|
401 |
|
|
402 |
if (wxGetKeyState(WXK_LEFT)) |
|
403 |
{ |
|
404 |
teleop_fluid_omega -= 1; |
|
405 |
} |
|
406 |
else if (wxGetKeyState(WXK_RIGHT)) |
|
407 |
{ |
|
408 |
teleop_fluid_omega += 1; |
|
409 |
} |
|
410 |
else if (teleop_fluid_omega > 0) |
|
411 |
{ |
|
412 |
teleop_fluid_omega -= 1; |
|
413 |
} |
|
414 |
else if (teleop_fluid_omega < 0) |
|
415 |
{ |
|
416 |
teleop_fluid_omega += 1; |
|
417 |
} |
|
418 |
|
|
419 |
if (teleop_fluid_speed > TELEOP_FLUID_MAX_SPEED) |
|
420 |
{ |
|
421 |
teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED; |
|
422 |
} |
|
423 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2) |
|
424 |
{ |
|
425 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2; |
|
426 |
} |
|
427 |
|
|
428 |
teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
429 |
teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
430 |
} |
|
431 |
|
|
432 |
void SimFrame::teleop() |
|
433 |
{ |
|
434 |
switch (teleop_type) |
|
435 |
{ |
|
436 |
case TELEOP_OFF: |
|
437 |
return; |
|
438 |
case TELEOP_PRECISE: |
|
439 |
teleop_move_precise(); |
|
440 |
break; |
|
441 |
case TELEOP_FLUID: |
|
442 |
teleop_move_fluid(); |
|
443 |
break; |
|
444 |
} |
|
445 |
|
|
446 |
motors::set_motors msg; |
|
447 |
msg.fl_set = true; |
|
448 |
msg.fr_set = true; |
|
449 |
msg.bl_set = true; |
|
450 |
msg.br_set = true; |
|
451 |
|
|
452 |
msg.fl_speed = teleop_l_speed; |
|
453 |
msg.fr_speed = teleop_r_speed; |
|
454 |
msg.bl_speed = teleop_l_speed; |
|
455 |
msg.br_speed = teleop_r_speed; |
|
456 |
|
|
457 |
teleop_pub.publish(msg); |
|
458 |
} |
|
459 |
|
|
318 | 460 |
void SimFrame::updateScouts() |
319 | 461 |
{ |
320 | 462 |
if (last_scout_update.isZero()) |
Also available in: Unified diff