Statistics
| Branch: | Revision:

root / scout / scoutsim / src / sim_frame.cpp @ 60a90290

History | View | Annotate | Download (19.1 KB)

1 c492be62 Alex Zirbel
/**
2
 * The code in this package was developed using the structure of Willow
3
 * Garage's turtlesim package.  It was modified by the CMU Robotics Club
4
 * to be used as a simulator for the Colony Scout robot.
5 266ae7f2 Alex Zirbel
 *
6 c492be62 Alex Zirbel
 * All redistribution of this code is limited to the terms of Willow Garage's
7
 * licensing terms, as well as under permission from the CMU Robotics Club.
8
 * 
9
 * Copyright (c) 2011 Colony Project
10
 * 
11
 * Permission is hereby granted, free of charge, to any person
12
 * obtaining a copy of this software and associated documentation
13
 * files (the "Software"), to deal in the Software without
14
 * restriction, including without limitation the rights to use,
15
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
16
 * copies of the Software, and to permit persons to whom the
17
 * Software is furnished to do so, subject to the following
18
 * conditions:
19
 * 
20
 * The above copyright notice and this permission notice shall be
21
 * included in all copies or substantial portions of the Software.
22
 * 
23
 * Copyright (c) 2009, Willow Garage, Inc.
24 266ae7f2 Alex Zirbel
 * All rights reserved.
25 c492be62 Alex Zirbel
 * 
26
 * Redistribution and use in source and binary forms, with or without
27
 * modification, are permitted provided that the following conditions are met:
28
 * 
29
 *    Redistributions of source code must retain the above copyright
30
 *       notice, this list of conditions and the following disclaimer.
31
 *    Redistributions in binary form must reproduce the above copyright
32
 *       notice, this list of conditions and the following disclaimer in the
33
 *       documentation and/or other materials provided with the distribution.
34
 *    Neither the name of the Willow Garage, Inc. nor the names of its
35
 *       contributors may be used to endorse or promote products derived from
36
 *       this software without specific prior written permission.
37
 * 
38
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
39
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
40
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
41
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
42
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
43
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
44
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
45
 * OTHER DEALINGS IN THE SOFTWARE.
46 266ae7f2 Alex Zirbel
 */
47
48 a8480867 Alex Zirbel
#include "sim_frame.h"
49 266ae7f2 Alex Zirbel
50 2eaafff2 Alex
#include <stdio.h>
51
52 266ae7f2 Alex Zirbel
#include <ros/package.h>
53
#include <cstdlib>
54
#include <ctime>
55
56 2eaafff2 Alex
using namespace std;
57 266ae7f2 Alex Zirbel
58
namespace scoutsim
59
{
60 2eaafff2 Alex
    SimFrame::SimFrame(wxWindow* parent, string map_name)
61 266ae7f2 Alex Zirbel
        : wxFrame(parent, wxID_ANY, wxT("ScoutSim"), wxDefaultPosition,
62
                  wxSize(500, 500), wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER)
63 144137a1 Alex Zirbel
          , frame_count(0)
64
          , id_counter(0)
65 266ae7f2 Alex Zirbel
    {
66 2eaafff2 Alex
        std::cout << "Constructing sim frame." << std::endl;
67
68 266ae7f2 Alex Zirbel
        srand(time(NULL));
69
70 144137a1 Alex Zirbel
        update_timer = new wxTimer(this);
71 7f095440 Priya
        update_timer->Start(REAL_TIME_REFRESH_RATE * 1000);
72 266ae7f2 Alex Zirbel
73 144137a1 Alex Zirbel
        Connect(update_timer->GetId(), wxEVT_TIMER,
74 266ae7f2 Alex Zirbel
                wxTimerEventHandler(SimFrame::onUpdate), NULL, this);
75
        Connect(wxEVT_PAINT, wxPaintEventHandler(SimFrame::onPaint),
76
                NULL, this);
77
78 594c3bb9 Alex
        images_path = ros::package::getPath("scoutsim") + "/images/";
79 266ae7f2 Alex Zirbel
80 ade1b7f9 Alex
        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
81
                           map_name + ".bmp";
82
        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
83
                           map_name + "_lines.bmp";
84 7ffad595 Alex
        map_walls_name = ros::package::getPath("scoutsim") + "/maps/" +
85
                           map_name + "_walls.bmp";
86 ade1b7f9 Alex
        display_map_name = map_base_name;
87
88
        wxBitmap lines_bitmap;
89 e2770306 Alex
        wxBitmap walls_bitmap;
90
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
91
92
        // Try to load the file; if it fails, make a new blank file
93
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
94
        {
95
            lines_bitmap = wxBitmap(path_bitmap.GetWidth(), path_bitmap.GetHeight(), 3);
96
        }
97 ade1b7f9 Alex
        lines_image = lines_bitmap.ConvertToImage();
98
99 e2770306 Alex
        // Try to load the file; if it fails, make a new blank file
100
        if (!walls_bitmap.LoadFile(wxString::FromAscii(map_walls_name.c_str())))
101
        {
102
            walls_bitmap = wxBitmap(path_bitmap.GetWidth(), path_bitmap.GetHeight(), 3);
103
        }
104 7ffad595 Alex
        walls_image = walls_bitmap.ConvertToImage();
105
106 266ae7f2 Alex Zirbel
        clear();
107
108 144137a1 Alex Zirbel
        clear_srv = nh.advertiseService("clear",
109 4612f7e4 Alex Zirbel
                                        &SimFrame::clearCallback, this);
110 144137a1 Alex Zirbel
        reset_srv = nh.advertiseService("reset",
111 4612f7e4 Alex Zirbel
                                        &SimFrame::resetCallback, this);
112 144137a1 Alex Zirbel
        spawn_srv = nh.advertiseService("spawn",
113 4612f7e4 Alex Zirbel
                                        &SimFrame::spawnCallback, this);
114 144137a1 Alex Zirbel
        kill_srv = nh.advertiseService("kill",
115 43811241 Alex
                                        &SimFrame::killCallback, this);
116
        set_sonar_viz_srv = nh.advertiseService("set_sonar_viz",
117
                                        &SimFrame::setSonarVizCallback, this);
118
        set_ghost_srv = nh.advertiseService("set_ghost",
119
                                        &SimFrame::setGhostCallback, this);
120
        set_teleop_srv = nh.advertiseService("set_teleop",
121
                                        &SimFrame::setTeleopCallback, this);
122 266ae7f2 Alex Zirbel
123 82f3f746 Priya
        // Subscribe and publisher wirless from robots
124 7ffad595 Alex
        wireless_receive = nh.advertise< ::messages::WirelessPacket>(
125
            "/wireless/receive", 1000); 
126
        wireless_send = nh.subscribe("/wireless/send", 1000,
127
            &SimFrame::wirelessCallback, this);
128 82f3f746 Priya
129 0e77683c Alex
        // Teleop
130 43811241 Alex
        teleop_type = TELEOP_OFF;
131 0e77683c Alex
        teleop_l_speed = 0;
132
        teleop_r_speed = 0;
133 60a90290 Hui Jun Tay
        teleop_scoutname = "scout1";
134
135 0e77683c Alex
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
136
137 266ae7f2 Alex Zirbel
        ROS_INFO("Starting scoutsim with node name %s",
138
                 ros::this_node::getName().c_str()) ;
139
140 2eaafff2 Alex
        wxMenu *menuFile = new wxMenu;
141
        menuFile->Append(ID_ABOUT, _("&About"));
142
        menuFile->AppendSeparator();
143
        menuFile->Append(ID_QUIT, _("E&xit"));
144
145
        wxMenu *menuSim = new wxMenu;
146
        menuSim->Append(ID_CLEAR, _("&Clear"));
147
148
        wxMenu *menuView = new wxMenu;
149
        menuView->Append(ID_MAP, _("&Map"));
150
        menuView->Append(ID_LINES, _("&Lines"));
151 7ffad595 Alex
        menuView->Append(ID_WALLS, _("&Walls"));
152 2eaafff2 Alex
153 0e77683c Alex
        wxMenu *menuTeleop = new wxMenu;
154
        menuTeleop->Append(ID_TELEOP_NONE, _("&None"));
155
        menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
156
        menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
157
158 2eaafff2 Alex
        wxMenuBar *menuBar = new wxMenuBar;
159
        menuBar->Append(menuFile, _("&File"));
160
        menuBar->Append(menuSim, _("&Sim"));
161
        menuBar->Append(menuView, _("&View"));
162 0e77683c Alex
        menuBar->Append(menuTeleop, _("&Teleop"));
163 2eaafff2 Alex
164
        SetMenuBar(menuBar);
165
166 c63c9752 Alex
        width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
167
        height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
168 25694a03 Priya
169
        spawnScout("scout1", 1.4, .78, 0);
170 266ae7f2 Alex Zirbel
    }
171
172
    SimFrame::~SimFrame()
173
    {
174 144137a1 Alex Zirbel
        delete update_timer;
175 266ae7f2 Alex Zirbel
    }
176
177 4612f7e4 Alex Zirbel
    bool SimFrame::spawnCallback(scoutsim::Spawn::Request  &req,
178
                                 scoutsim::Spawn::Response &res)
179 266ae7f2 Alex Zirbel
    {
180
        std::string name = spawnScout(req.name, req.x, req.y, req.theta);
181
        if (name.empty())
182
        {
183 43811241 Alex
            ROS_WARN("A scout named [%s] already exists", req.name.c_str());
184 266ae7f2 Alex Zirbel
            return false;
185
        }
186
187
        res.name = name;
188
        return true;
189
    }
190
191
    bool SimFrame::killCallback(scoutsim::Kill::Request& req,
192
                                scoutsim::Kill::Response&)
193
    {
194 144137a1 Alex Zirbel
        M_Scout::iterator it = scouts.find(req.name);
195
        if (it == scouts.end())
196 266ae7f2 Alex Zirbel
        {
197 43811241 Alex
            ROS_WARN("Tried to kill scout [%s], which does not exist",
198
                     req.name.c_str());
199 266ae7f2 Alex Zirbel
            return false;
200
        }
201
202 144137a1 Alex Zirbel
        scouts.erase(it);
203 266ae7f2 Alex Zirbel
204
        return true;
205
    }
206
207 43811241 Alex
    bool SimFrame::setSonarVizCallback(SetSonarViz::Request& req,
208
                                       SetSonarViz::Response&)
209
    {
210
        M_Scout::iterator it = scouts.find(req.scout_name);
211
        if (it == scouts.end())
212
        {
213
            ROS_WARN("Tried to set sonar on scout [%s], which does not exist",
214
                     req.scout_name.c_str());
215
            return false;
216
        }
217
218
        it->second->set_sonar_visual(req.on);
219
        return true;
220
    }
221
222
    bool SimFrame::setGhostCallback(SetGhost::Request& req,
223
                                    SetGhost::Response&)
224
    {
225
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
226
        {
227
            if (ghost_scouts.at(i)->get_name() == req.scout_name)
228
            {
229
                ghost_scouts.at(i)->set_visible(req.on);
230
                return true;
231
            }
232
        }
233
234
        ROS_WARN("Tried to set ghost on scout [%s], which does not exist",
235
                 req.scout_name.c_str());
236
        return false;
237
    }
238
239
    bool SimFrame::setTeleopCallback(SetTeleop::Request& req,
240
                                     SetTeleop::Response&)
241
    {
242
        std::stringstream ss;
243
        ss << "/" << req.scout_name << "/set_motors";
244
        teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
245 60a90290 Hui Jun Tay
        teleop_scoutname = req.scout_name.c_str();
246
        ROS_INFO("Teleoping %s...",teleop_scoutname.c_str()); //debug statement
247 43811241 Alex
248
        return true;
249
    }
250
251 266ae7f2 Alex Zirbel
    bool SimFrame::hasScout(const std::string& name)
252
    {
253 144137a1 Alex Zirbel
        return scouts.find(name) != scouts.end();
254 266ae7f2 Alex Zirbel
    }
255
256 9b3564f3 Alex Zirbel
    std::string SimFrame::spawnScout(const std::string& name,
257
                                     float x, float y, float angle)
258 266ae7f2 Alex Zirbel
    {
259
        std::string real_name = name;
260
        if (real_name.empty())
261
        {
262 43811241 Alex
            // Generate the name scoutX, where X is an increasing number.
263 266ae7f2 Alex Zirbel
            do
264
            {
265
                std::stringstream ss;
266 144137a1 Alex Zirbel
                ss << "scout" << ++id_counter;
267 266ae7f2 Alex Zirbel
                real_name = ss.str();
268 43811241 Alex
            }
269
            while (hasScout(real_name));
270 266ae7f2 Alex Zirbel
        }
271
        else
272
        {
273
            if (hasScout(real_name))
274
            {
275
                return "";
276
            }
277
        }
278
279 594c3bb9 Alex
        wxImage scout_image;
280
281
        // Try to load a name-specific image; if not, load the default scout
282
        string specific_name = images_path + name + ".png";
283
        if (fileExists(specific_name))
284
        {
285
            scout_image.LoadFile(wxString::FromAscii(specific_name.c_str()));
286
            scout_image.SetMask(true);
287
            scout_image.SetMaskColour(255, 255, 255);
288
        }
289
        else
290
        {
291
            scout_image.LoadFile(
292
                wxString::FromAscii((images_path + "scout.png").c_str()));
293
            scout_image.SetMask(true);
294
            scout_image.SetMaskColour(255, 255, 255);
295
        }
296
297 266ae7f2 Alex Zirbel
        ScoutPtr t(new Scout(ros::NodeHandle(real_name),
298 594c3bb9 Alex
                   scout_image, Vector2(x, y), &path_bitmap, angle));
299 144137a1 Alex Zirbel
        scouts[real_name] = t;
300 266ae7f2 Alex Zirbel
301 0e0ef125 Priya
        ghost_scouts.push_back(new GhostScout(ros::NodeHandle(real_name),
302
                scout_image, Vector2(x, y), &path_bitmap, angle, name));
303
304 266ae7f2 Alex Zirbel
        ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",
305
                 real_name.c_str(), x, y, angle);
306
307
        return real_name;
308
    }
309
310 2eaafff2 Alex
    void SimFrame::onQuit(wxCommandEvent& WXUNUSED(event))
311
    {
312
        Close(true);
313
    }
314
315
    void SimFrame::onAbout(wxCommandEvent& WXUNUSED(event))
316
    {
317
        wxMessageBox(_("Scoutsim is the simulator the Colony Project's scout robot.\n"
318
                       "\nThe Colony Project is a part of the Carnegie Mellon\n"
319
                       "Robotics Club. Our goal is to use cooperative low-cost\n"
320
                       "robots to solve challenging problems."),
321
                     _("About Scoutsim"),
322
                     wxOK | wxICON_INFORMATION, this );
323
    }
324
325
    void SimFrame::onClear(wxCommandEvent& WXUNUSED(event))
326 266ae7f2 Alex Zirbel
    {
327 2eaafff2 Alex
        clear();
328
    }
329 266ae7f2 Alex Zirbel
330 2eaafff2 Alex
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
331
    {
332 ade1b7f9 Alex
        display_map_name = map_base_name;
333 2eaafff2 Alex
        clear();
334
    }
335 266ae7f2 Alex Zirbel
336 2eaafff2 Alex
    void SimFrame::showLines(wxCommandEvent& WXUNUSED(event))
337
    {
338 ade1b7f9 Alex
        display_map_name = map_lines_name;
339 2eaafff2 Alex
        clear();
340
    }
341 7ffad595 Alex
    
342
    void SimFrame::showWalls(wxCommandEvent& WXUNUSED(event))
343
    {
344
        display_map_name = map_walls_name;
345
        clear();
346
    }
347 2eaafff2 Alex
348
    void SimFrame::clear()
349
    {
350
        path_dc.SetBackground(wxBrush(wxColour(100, 100, 100)));
351 144137a1 Alex Zirbel
        path_dc.Clear();
352 2eaafff2 Alex
353 6639ce9c viki
        sonar_dc.SetBackground(wxBrush(wxColour(255, 0, 0)));
354
        sonar_dc.Clear();
355
356
        sonar_dc.SelectObject(path_bitmap);
357
358 2eaafff2 Alex
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
359
        path_dc.SelectObject(path_bitmap);
360
        SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
361 266ae7f2 Alex Zirbel
    }
362
363 7f095440 Priya
    // Runs every REAL_TIME_REFRESH_RATE.
364 266ae7f2 Alex Zirbel
    void SimFrame::onUpdate(wxTimerEvent& evt)
365
    {
366
        ros::spinOnce();
367
368 0e77683c Alex
        teleop();
369
370 266ae7f2 Alex Zirbel
        updateScouts();
371
372
        if (!ros::ok())
373
        {
374
            Close();
375
        }
376 04114d13 Alex
377
        frame_count++;
378 266ae7f2 Alex Zirbel
    }
379
380
    void SimFrame::onPaint(wxPaintEvent& evt)
381
    {
382
        wxPaintDC dc(this);
383
384 144137a1 Alex Zirbel
        dc.DrawBitmap(path_bitmap, 0, 0, true);
385 266ae7f2 Alex Zirbel
386 144137a1 Alex Zirbel
        M_Scout::iterator it = scouts.begin();
387
        M_Scout::iterator end = scouts.end();
388 266ae7f2 Alex Zirbel
        for (; it != end; ++it)
389
        {
390
            it->second->paint(dc);
391
        }
392 43811241 Alex
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
393 0e0ef125 Priya
        {
394
            ghost_scouts.at(i)->paint(dc);
395
        }
396 266ae7f2 Alex Zirbel
    }
397
398 594c3bb9 Alex
    bool SimFrame::fileExists(const std::string& filename)
399
    {
400
        struct stat buf;
401
        if (stat(filename.c_str(), &buf) != -1)
402
        {
403
            return true;
404
        }
405
        return false;
406
    }
407
408 0e77683c Alex
    void SimFrame::stopTeleop(wxCommandEvent& event)
409
    {
410
        teleop_type = TELEOP_OFF;
411
        teleop_l_speed = 0;
412
        teleop_r_speed = 0;
413
    }
414
415
    void SimFrame::startTeleopPrecise(wxCommandEvent& event)
416
    {
417
        teleop_type = TELEOP_PRECISE;
418
        teleop_l_speed = 0;
419
        teleop_r_speed = 0;
420
    }
421
422
    void SimFrame::startTeleopFluid(wxCommandEvent& event)
423
    {
424
        teleop_type = TELEOP_FLUID;
425
        teleop_l_speed = 0;
426
        teleop_r_speed = 0;
427
        teleop_fluid_speed = 0;
428
        teleop_fluid_omega = 0;
429
    }
430
431
    void SimFrame::teleop_move_precise()
432
    {
433
        // Default to stop
434
        teleop_l_speed = 0;
435
        teleop_r_speed = 0;
436
437
        if (wxGetKeyState(WXK_UP))
438
        {
439
            teleop_l_speed = TELEOP_PRECISE_SPEED;
440
            teleop_r_speed = TELEOP_PRECISE_SPEED;
441
        }
442
        else if (wxGetKeyState(WXK_DOWN))
443
        {
444
            teleop_l_speed = -TELEOP_PRECISE_SPEED;
445
            teleop_r_speed = -TELEOP_PRECISE_SPEED;
446
        }
447
        else if (wxGetKeyState(WXK_LEFT))
448
        {
449 43811241 Alex
            teleop_l_speed = -TELEOP_PRECISE_TURN_SPEED;
450
            teleop_r_speed = TELEOP_PRECISE_TURN_SPEED;
451 0e77683c Alex
        }
452
        else if (wxGetKeyState(WXK_RIGHT))
453
        {
454 43811241 Alex
            teleop_l_speed = TELEOP_PRECISE_TURN_SPEED;
455
            teleop_r_speed = -TELEOP_PRECISE_TURN_SPEED;
456 0e77683c Alex
        }
457
    }
458
459
    void SimFrame::teleop_move_fluid()
460
    {
461
        if (wxGetKeyState(WXK_UP))
462
        {
463 c63c9752 Alex
            teleop_fluid_speed += TELEOP_FLUID_INC * 2;
464 0e77683c Alex
        }
465
        else if (wxGetKeyState(WXK_DOWN))
466
        {
467 c63c9752 Alex
            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
468 0e77683c Alex
        }
469 04114d13 Alex
        else if (teleop_fluid_speed > TELEOP_FLUID_INC)
470 0e77683c Alex
        {
471 c63c9752 Alex
            teleop_fluid_speed -= TELEOP_FLUID_INC;
472 0e77683c Alex
        }
473 04114d13 Alex
        else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
474 0e77683c Alex
        {
475 c63c9752 Alex
            teleop_fluid_speed += TELEOP_FLUID_INC;
476 0e77683c Alex
        }
477 04114d13 Alex
        else
478
        {
479
            teleop_fluid_speed = 0;
480
        }
481 0e77683c Alex
482
        if (wxGetKeyState(WXK_LEFT))
483
        {
484 7f095440 Priya
            teleop_fluid_omega -= TELEOP_FLUID_INC * 2;
485 0e77683c Alex
        }
486
        else if (wxGetKeyState(WXK_RIGHT))
487
        {
488 7f095440 Priya
            teleop_fluid_omega += TELEOP_FLUID_INC * 2;
489 0e77683c Alex
        }
490 0076084e Alex
        else
491 0e77683c Alex
        {
492 0076084e Alex
            teleop_fluid_omega = 0;
493 0e77683c Alex
        }
494
495
        if (teleop_fluid_speed > TELEOP_FLUID_MAX_SPEED)
496
        {
497
            teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
498
        }
499 c63c9752 Alex
        else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED)
500
        {
501
            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
502
        }
503 04114d13 Alex
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED)
504 0e77683c Alex
        {
505 04114d13 Alex
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED;
506 0e77683c Alex
        }
507 04114d13 Alex
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED)
508 c63c9752 Alex
        {
509 04114d13 Alex
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED;
510 c63c9752 Alex
        }
511 0e77683c Alex
512 04114d13 Alex
        int l_speed = teleop_fluid_speed + teleop_fluid_omega;
513
        int r_speed = teleop_fluid_speed - teleop_fluid_omega;
514
515
        teleop_l_speed = max(MIN_ABSOLUTE_SPEED,
516
                             min(MAX_ABSOLUTE_SPEED, l_speed));
517
        teleop_r_speed = max(MIN_ABSOLUTE_SPEED,
518
                             min(MAX_ABSOLUTE_SPEED, r_speed));
519 0e77683c Alex
    }
520
521
    void SimFrame::teleop()
522
    {
523
        switch (teleop_type)
524
        {
525
            case TELEOP_OFF:
526
                return;
527
            case TELEOP_PRECISE:
528
                teleop_move_precise();
529
                break;
530
            case TELEOP_FLUID:
531
                teleop_move_fluid();
532
                break;
533
        }
534
535
        motors::set_motors msg;
536
        msg.fl_set = true;
537
        msg.fr_set = true;
538
        msg.bl_set = true;
539
        msg.br_set = true;
540 60a90290 Hui Jun Tay
        msg.teleop_ON = true;
541 0e77683c Alex
542
        msg.fl_speed = teleop_l_speed;
543
        msg.fr_speed = teleop_r_speed;
544
        msg.bl_speed = teleop_l_speed;
545
        msg.br_speed = teleop_r_speed;
546
547
        teleop_pub.publish(msg);
548
    }
549
550 266ae7f2 Alex Zirbel
    void SimFrame::updateScouts()
551
    {
552 60a90290 Hui Jun Tay
        std::string teleop_out = "";
553
554 144137a1 Alex Zirbel
        if (last_scout_update.isZero())
555 266ae7f2 Alex Zirbel
        {
556 144137a1 Alex Zirbel
            last_scout_update = ros::WallTime::now();
557 266ae7f2 Alex Zirbel
            return;
558
        }
559
560 04114d13 Alex
        path_image = path_bitmap.ConvertToImage();
561
        Refresh();
562 266ae7f2 Alex Zirbel
563 144137a1 Alex Zirbel
        M_Scout::iterator it = scouts.begin();
564
        M_Scout::iterator end = scouts.end();
565 e3f69e61 Alex
566
        world_state state;
567
        state.canvas_width = width_in_meters;
568
        state.canvas_height = height_in_meters;
569
570 266ae7f2 Alex Zirbel
        for (; it != end; ++it)
571
        {
572 60a90290 Hui Jun Tay
573
        //default to scout1
574
        if (teleop_type != TELEOP_OFF)
575
        {
576
            teleop_out = teleop_scoutname;
577
        }
578
579 7f095440 Priya
            it->second->update(SIM_TIME_REFRESH_RATE,
580 43811241 Alex
                               path_dc, sonar_dc,
581 a2e6bd4c Alex
                               path_image, lines_image, walls_image,
582 144137a1 Alex Zirbel
                               path_dc.GetBackground().GetColour(),
583 04114d13 Alex
                               sonar_dc.GetBackground().GetColour(),
584 60a90290 Hui Jun Tay
                               state,teleop_out);
585 266ae7f2 Alex Zirbel
        }
586
587 43811241 Alex
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
588 0e0ef125 Priya
        {
589 7f095440 Priya
            ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
590 0e0ef125 Priya
                path_dc.GetBackground().GetColour(), state);
591
        }
592
593 c492be62 Alex Zirbel
        frame_count++;
594 266ae7f2 Alex Zirbel
    }
595
596
    bool SimFrame::clearCallback(std_srvs::Empty::Request&,
597
                                 std_srvs::Empty::Response&)
598
    {
599
        ROS_INFO("Clearing scoutsim.");
600
        clear();
601
        return true;
602
    }
603
604
    bool SimFrame::resetCallback(std_srvs::Empty::Request&,
605
                                 std_srvs::Empty::Response&)
606
    {
607
        ROS_INFO("Resetting scoutsim.");
608 144137a1 Alex Zirbel
        scouts.clear();
609
        id_counter = 0;
610
        spawnScout("", width_in_meters / 2.0, height_in_meters / 2.0, 0);
611 266ae7f2 Alex Zirbel
        clear();
612
        return true;
613
    }
614
615 1d1281cc Priya
    void SimFrame::wirelessCallback(const ::messages::WirelessPacket::ConstPtr& msg)
616 82f3f746 Priya
    {
617
        wireless_receive.publish(msg);
618
    }
619 266ae7f2 Alex Zirbel
}