Revision 0e77683c
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