Revision 0e77683c scout/scoutsim/src/sim_frame.cpp

View differences:

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