Project

General

Profile

Revision 0e77683c

ID0e77683c65f392af332f84aa9b31eb8614290a55
Parent 759ac4fc
Child 812788aa

Added by Alex Zirbel over 11 years ago

Added built-in scoutsim teleop!

View differences:

scout/scoutsim/src/scoutsim.cpp
134 134
    EVT_MENU(ID_MAP, scoutsim::SimFrame::showMap)
135 135
    EVT_MENU(ID_LINES, scoutsim::SimFrame::showLines)
136 136
    EVT_MENU(ID_WALLS, scoutsim::SimFrame::showWalls)
137
    EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop)
138
    EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise)
139
    EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid)
137 140
END_EVENT_TABLE()
138 141

  
139 142
DECLARE_APP(ScoutApp);
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())
scout/scoutsim/src/sim_frame.h
49 49
#include <wx/event.h>
50 50
#include <wx/timer.h>
51 51
#include <wx/string.h>
52
#include <wx/utils.h>
52 53

  
53 54
#include <ros/ros.h>
54 55

  
55 56
#include <std_srvs/Empty.h>
56 57
#include <scoutsim/Spawn.h>
57 58
#include <scoutsim/Kill.h>
59
#include <motors/set_motors.h>
58 60
#include <map>
59 61

  
60 62
#include "scout.h"
......
68 70
#define ID_MAP 4
69 71
#define ID_LINES 5
70 72
#define ID_WALLS 6
73
#define ID_TELEOP_NONE 7
74
#define ID_TELEOP_PRECISE 8
75
#define ID_TELEOP_FLUID 9
76

  
77
#define TELEOP_PRECISE_SPEED 30
78
#define TELEOP_FLUID_MAX_SPEED 80
79

  
80
// Teleop types
81
#define TELEOP_OFF 0
82
#define TELEOP_PRECISE 1
83
#define TELEOP_FLUID 2
71 84

  
72 85
namespace scoutsim
73 86
{
......
86 99
            void showMap(wxCommandEvent& event);
87 100
            void showLines(wxCommandEvent& event);
88 101
            void showWalls(wxCommandEvent& event);
102
            void stopTeleop(wxCommandEvent& event);
103
            void startTeleopPrecise(wxCommandEvent& event);
104
            void startTeleopFluid(wxCommandEvent& event);
89 105

  
90 106
            DECLARE_EVENT_TABLE()
91 107

  
......
93 109
            void onUpdate(wxTimerEvent& evt);
94 110
            void onPaint(wxPaintEvent& evt);
95 111

  
112
            void teleop_move_precise();
113
            void teleop_move_fluid();
114
            void teleop();
115

  
96 116
            void updateScouts();
97 117
            void clear();
98 118
            bool hasScout(const std::string& name);
......
107 127
            ros::Subscriber wireless_send;
108 128
            ros::Publisher wireless_receive;
109 129

  
130
            // Teleop
131
            short teleop_l_speed;
132
            short teleop_r_speed;
133
            ros::Publisher teleop_pub;
134
            int teleop_type;
135

  
136
            short teleop_fluid_speed;
137
            short teleop_fluid_omega;
138

  
110 139
            ros::NodeHandle nh;
111 140
            wxTimer* update_timer;
112 141
            wxBitmap path_bitmap;
......
114 143
            wxImage lines_image;
115 144
            wxImage walls_image;
116 145
            wxMemoryDC path_dc;
117
	    wxMemoryDC sonar_dc;
146
            wxMemoryDC sonar_dc;
118 147

  
119 148
            uint64_t frame_count;
120 149

  

Also available in: Unified diff