Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / src / sim_frame.cpp @ 3a73516c

History | View | Annotate | Download (19 KB)

1
/**
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
 *
6
 * 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
 * All rights reserved.
25
 * 
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
 */
47

    
48
/**
49
 * @file sim_frame.cpp
50
 *
51
 * @ingroup scoutsim
52
 * @{
53
 */
54

    
55
#include "sim_frame.h"
56

    
57
#include <stdio.h>
58

    
59
#include <ros/package.h>
60
#include <cstdlib>
61
#include <ctime>
62

    
63
using namespace std;
64

    
65
namespace scoutsim
66
{
67
    SimFrame::SimFrame(wxWindow* parent, string map_name)
68
        : wxFrame(parent, wxID_ANY, wxT("ScoutSim"), wxDefaultPosition,
69
                  wxSize(500, 500), wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER)
70
          , frame_count(0)
71
          , id_counter(0)
72
    {
73
        std::cout << "Constructing sim frame." << std::endl;
74

    
75
        srand(time(NULL));
76

    
77
        update_timer = new wxTimer(this);
78
        update_timer->Start(REAL_TIME_REFRESH_RATE * 1000);
79

    
80
        Connect(update_timer->GetId(), wxEVT_TIMER,
81
                wxTimerEventHandler(SimFrame::onUpdate), NULL, this);
82
        Connect(wxEVT_PAINT, wxPaintEventHandler(SimFrame::onPaint),
83
                NULL, this);
84

    
85
        images_path = ros::package::getPath("scoutsim") + "/images/";
86

    
87
        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
88
                           map_name + ".bmp";
89
        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
90
                           map_name + "_lines.bmp";
91
        map_walls_name = ros::package::getPath("scoutsim") + "/maps/" +
92
                           map_name + "_walls.bmp";
93
        display_map_name = map_base_name;
94

    
95
        wxBitmap lines_bitmap;
96
        wxBitmap walls_bitmap;
97
        ROS_INFO("Loading map: %s", display_map_name.c_str());
98
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
99

    
100
        // Try to load the file; if it fails, make a new blank file
101
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
102
        {
103
            lines_bitmap = wxBitmap(path_bitmap.GetWidth(), path_bitmap.GetHeight(), 3);
104
        }
105
        lines_image = lines_bitmap.ConvertToImage();
106

    
107
        // Try to load the file; if it fails, make a new blank file
108
        if (!walls_bitmap.LoadFile(wxString::FromAscii(map_walls_name.c_str())))
109
        {
110
            walls_bitmap = wxBitmap(path_bitmap.GetWidth(), path_bitmap.GetHeight(), 3);
111
        }
112
        walls_image = walls_bitmap.ConvertToImage();
113

    
114
        clear();
115

    
116
        clear_srv = nh.advertiseService("clear",
117
                                        &SimFrame::clearCallback, this);
118
        reset_srv = nh.advertiseService("reset",
119
                                        &SimFrame::resetCallback, this);
120
        spawn_srv = nh.advertiseService("spawn",
121
                                        &SimFrame::spawnCallback, this);
122
        kill_srv = nh.advertiseService("kill",
123
                                        &SimFrame::killCallback, this);
124
        set_sonar_viz_srv = nh.advertiseService("set_sonar_viz",
125
                                        &SimFrame::setSonarVizCallback, this);
126
        set_ghost_srv = nh.advertiseService("set_ghost",
127
                                        &SimFrame::setGhostCallback, this);
128
        set_teleop_srv = nh.advertiseService("set_teleop",
129
                                        &SimFrame::setTeleopCallback, this);
130

    
131
        // Subscribe and publisher wirless from robots
132
        wireless_receive = nh.advertise< ::messages::WirelessPacket>(
133
            "/wireless/receive", 1000); 
134
        wireless_send = nh.subscribe("/wireless/send", 1000,
135
            &SimFrame::wirelessCallback, this);
136

    
137
        // Teleop
138
        teleop_type = TELEOP_OFF;
139
        teleop_l_speed = 0;
140
        teleop_r_speed = 0;
141
        teleop_scoutname = "scout1";
142

    
143
        teleop_pub = nh.advertise< ::messages::set_motors>("/scout1/set_motors", 1000);
144

    
145
        ROS_INFO("Starting scoutsim with node name %s",
146
                 ros::this_node::getName().c_str()) ;
147

    
148
        wxMenu *menuFile = new wxMenu;
149
        menuFile->Append(ID_ABOUT, _("&About"));
150
        menuFile->AppendSeparator();
151
        menuFile->Append(ID_QUIT, _("E&xit"));
152

    
153
        wxMenu *menuSim = new wxMenu;
154
        menuSim->Append(ID_CLEAR, _("&Clear"));
155

    
156
        wxMenu *menuView = new wxMenu;
157
        menuView->Append(ID_MAP, _("&Map"));
158
        menuView->Append(ID_LINES, _("&Lines"));
159
        menuView->Append(ID_WALLS, _("&Walls"));
160

    
161
        wxMenu *menuTeleop = new wxMenu;
162
        menuTeleop->Append(ID_TELEOP_NONE, _("&None"));
163
        menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
164
        menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
165

    
166
        wxMenuBar *menuBar = new wxMenuBar;
167
        menuBar->Append(menuFile, _("&File"));
168
        menuBar->Append(menuSim, _("&Sim"));
169
        menuBar->Append(menuView, _("&View"));
170
        menuBar->Append(menuTeleop, _("&Teleop"));
171

    
172
        SetMenuBar(menuBar);
173

    
174
        width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
175
        height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
176

    
177
        spawnScout("scout1", 1.4, .78, 0);
178
    }
179

    
180
    SimFrame::~SimFrame()
181
    {
182
        delete update_timer;
183
    }
184

    
185
    bool SimFrame::spawnCallback(scoutsim::Spawn::Request  &req,
186
                                 scoutsim::Spawn::Response &res)
187
    {
188
        std::string name = spawnScout(req.name, req.x, req.y, req.theta);
189
        if (name.empty())
190
        {
191
            ROS_WARN("A scout named [%s] already exists", req.name.c_str());
192
            return false;
193
        }
194

    
195
        res.name = name;
196
        return true;
197
    }
198

    
199
    bool SimFrame::killCallback(scoutsim::Kill::Request& req,
200
                                scoutsim::Kill::Response&)
201
    {
202
        M_Scout::iterator it = scouts.find(req.name);
203
        if (it == scouts.end())
204
        {
205
            ROS_WARN("Tried to kill scout [%s], which does not exist",
206
                     req.name.c_str());
207
            return false;
208
        }
209

    
210
        scouts.erase(it);
211

    
212
        return true;
213
    }
214

    
215
    bool SimFrame::setSonarVizCallback(SetSonarViz::Request& req,
216
                                       SetSonarViz::Response&)
217
    {
218
        M_Scout::iterator it = scouts.find(req.scout_name);
219
        if (it == scouts.end())
220
        {
221
            ROS_WARN("Tried to set sonar on scout [%s], which does not exist",
222
                     req.scout_name.c_str());
223
            return false;
224
        }
225

    
226
        it->second->set_sonar_visual(req.on);
227
        return true;
228
    }
229

    
230
    bool SimFrame::setGhostCallback(SetGhost::Request& req,
231
                                    SetGhost::Response&)
232
    {
233
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
234
        {
235
            if (ghost_scouts.at(i)->get_name() == req.scout_name)
236
            {
237
                ghost_scouts.at(i)->set_visible(req.on);
238
                return true;
239
            }
240
        }
241

    
242
        ROS_WARN("Tried to set ghost on scout [%s], which does not exist",
243
                 req.scout_name.c_str());
244
        return false;
245
    }
246

    
247
    bool SimFrame::setTeleopCallback(SetTeleop::Request& req,
248
                                     SetTeleop::Response&)
249
    {
250
        std::stringstream ss;
251
        ss << "/" << req.scout_name << "/set_motors";
252
        teleop_pub = nh.advertise< ::messages::set_motors>(ss.str(), 1000);
253
        teleop_scoutname = req.scout_name.c_str();
254
        ROS_INFO("Teleoping %s...",teleop_scoutname.c_str()); //debug statement
255

    
256
        return true;
257
    }
258

    
259
    bool SimFrame::hasScout(const std::string& name)
260
    {
261
        return scouts.find(name) != scouts.end();
262
    }
263

    
264
    std::string SimFrame::spawnScout(const std::string& name,
265
                                     float x, float y, float angle)
266
    {
267
        std::string real_name = name;
268
        if (real_name.empty())
269
        {
270
            // Generate the name scoutX, where X is an increasing number.
271
            do
272
            {
273
                std::stringstream ss;
274
                ss << "scout" << ++id_counter;
275
                real_name = ss.str();
276
            }
277
            while (hasScout(real_name));
278
        }
279
        else
280
        {
281
            if (hasScout(real_name))
282
            {
283
                return "";
284
            }
285
        }
286

    
287
        wxImage scout_image;
288

    
289
        // Try to load a name-specific image; if not, load the default scout
290
        string specific_name = images_path + name + ".png";
291
        if (fileExists(specific_name))
292
        {
293
            scout_image.LoadFile(wxString::FromAscii(specific_name.c_str()));
294
            scout_image.SetMask(true);
295
            scout_image.SetMaskColour(255, 255, 255);
296
        }
297
        else
298
        {
299
            scout_image.LoadFile(
300
                wxString::FromAscii((images_path + "scout.png").c_str()));
301
            scout_image.SetMask(true);
302
            scout_image.SetMaskColour(255, 255, 255);
303
        }
304

    
305
        ScoutPtr t(new Scout(ros::NodeHandle(real_name),
306
                   scout_image, Vector2(x, y), &path_bitmap, angle));
307
        scouts[real_name] = t;
308

    
309
        ghost_scouts.push_back(new GhostScout(ros::NodeHandle(real_name),
310
                scout_image, Vector2(x, y), &path_bitmap, angle, name));
311

    
312
        ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",
313
                 real_name.c_str(), x, y, angle);
314

    
315
        return real_name;
316
    }
317

    
318
    void SimFrame::onQuit(wxCommandEvent& WXUNUSED(event))
319
    {
320
        Close(true);
321
    }
322

    
323
    void SimFrame::onAbout(wxCommandEvent& WXUNUSED(event))
324
    {
325
        wxMessageBox(_("Scoutsim is the simulator the Colony Project's scout robot.\n"
326
                       "\nThe Colony Project is a part of the Carnegie Mellon\n"
327
                       "Robotics Club. Our goal is to use cooperative low-cost\n"
328
                       "robots to solve challenging problems."),
329
                     _("About Scoutsim"),
330
                     wxOK | wxICON_INFORMATION, this );
331
    }
332

    
333
    void SimFrame::onClear(wxCommandEvent& WXUNUSED(event))
334
    {
335
        clear();
336
    }
337

    
338
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
339
    {
340
        display_map_name = map_base_name;
341
        clear();
342
    }
343

    
344
    void SimFrame::showLines(wxCommandEvent& WXUNUSED(event))
345
    {
346
        display_map_name = map_lines_name;
347
        clear();
348
    }
349
    
350
    void SimFrame::showWalls(wxCommandEvent& WXUNUSED(event))
351
    {
352
        display_map_name = map_walls_name;
353
        clear();
354
    }
355

    
356
    void SimFrame::clear()
357
    {
358
        path_dc.SetBackground(wxBrush(wxColour(100, 100, 100)));
359
        path_dc.Clear();
360

    
361
        sonar_dc.SetBackground(wxBrush(wxColour(255, 0, 0)));
362
        sonar_dc.Clear();
363

    
364
        sonar_dc.SelectObject(path_bitmap);
365

    
366
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
367
        path_dc.SelectObject(path_bitmap);
368
        SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
369
    }
370

    
371
    // Runs every REAL_TIME_REFRESH_RATE.
372
    void SimFrame::onUpdate(wxTimerEvent& evt)
373
    {
374
        ros::spinOnce();
375

    
376
        teleop();
377

    
378
        updateScouts();
379

    
380
        if (!ros::ok())
381
        {
382
            Close();
383
        }
384

    
385
        frame_count++;
386
    }
387

    
388
    void SimFrame::onPaint(wxPaintEvent& evt)
389
    {
390
        wxPaintDC dc(this);
391

    
392
        dc.DrawBitmap(path_bitmap, 0, 0, true);
393

    
394
        M_Scout::iterator it = scouts.begin();
395
        M_Scout::iterator end = scouts.end();
396
        for (; it != end; ++it)
397
        {
398
            it->second->paint(dc);
399
        }
400
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
401
        {
402
            ghost_scouts.at(i)->paint(dc);
403
        }
404
    }
405

    
406
    bool SimFrame::fileExists(const std::string& filename)
407
    {
408
        struct stat buf;
409
        if (stat(filename.c_str(), &buf) != -1)
410
        {
411
            return true;
412
        }
413
        return false;
414
    }
415

    
416
    void SimFrame::stopTeleop(wxCommandEvent& event)
417
    {
418
        teleop_type = TELEOP_OFF;
419
        teleop_l_speed = 0;
420
        teleop_r_speed = 0;
421
    }
422

    
423
    void SimFrame::startTeleopPrecise(wxCommandEvent& event)
424
    {
425
        teleop_type = TELEOP_PRECISE;
426
        teleop_l_speed = 0;
427
        teleop_r_speed = 0;
428
    }
429

    
430
    void SimFrame::startTeleopFluid(wxCommandEvent& event)
431
    {
432
        teleop_type = TELEOP_FLUID;
433
        teleop_l_speed = 0;
434
        teleop_r_speed = 0;
435
        teleop_fluid_speed = 0;
436
        teleop_fluid_omega = 0;
437
    }
438

    
439
    void SimFrame::teleop_move_precise()
440
    {
441
        // Default to stop
442
        teleop_l_speed = 0;
443
        teleop_r_speed = 0;
444

    
445
        if (wxGetKeyState(WXK_UP))
446
        {
447
            teleop_l_speed = TELEOP_PRECISE_SPEED;
448
            teleop_r_speed = TELEOP_PRECISE_SPEED;
449
        }
450
        else if (wxGetKeyState(WXK_DOWN))
451
        {
452
            teleop_l_speed = -TELEOP_PRECISE_SPEED;
453
            teleop_r_speed = -TELEOP_PRECISE_SPEED;
454
        }
455
        else if (wxGetKeyState(WXK_LEFT))
456
        {
457
            teleop_l_speed = -TELEOP_PRECISE_TURN_SPEED;
458
            teleop_r_speed = TELEOP_PRECISE_TURN_SPEED;
459
        }
460
        else if (wxGetKeyState(WXK_RIGHT))
461
        {
462
            teleop_l_speed = TELEOP_PRECISE_TURN_SPEED;
463
            teleop_r_speed = -TELEOP_PRECISE_TURN_SPEED;
464
        }
465
    }
466

    
467
    void SimFrame::teleop_move_fluid()
468
    {
469
        if (wxGetKeyState(WXK_UP))
470
        {
471
            teleop_fluid_speed += TELEOP_FLUID_INC * 2;
472
        }
473
        else if (wxGetKeyState(WXK_DOWN))
474
        {
475
            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
476
        }
477
        else if (teleop_fluid_speed > TELEOP_FLUID_INC)
478
        {
479
            teleop_fluid_speed -= TELEOP_FLUID_INC;
480
        }
481
        else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
482
        {
483
            teleop_fluid_speed += TELEOP_FLUID_INC;
484
        }
485
        else
486
        {
487
            teleop_fluid_speed = 0;
488
        }
489

    
490
        if (wxGetKeyState(WXK_LEFT))
491
        {
492
            teleop_fluid_omega -= TELEOP_FLUID_INC * 2;
493
        }
494
        else if (wxGetKeyState(WXK_RIGHT))
495
        {
496
            teleop_fluid_omega += TELEOP_FLUID_INC * 2;
497
        }
498
        else
499
        {
500
            teleop_fluid_omega = 0;
501
        }
502

    
503
        if (teleop_fluid_speed > TELEOP_FLUID_MAX_SPEED)
504
        {
505
            teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
506
        }
507
        else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED)
508
        {
509
            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
510
        }
511
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED)
512
        {
513
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED;
514
        }
515
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED)
516
        {
517
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED;
518
        }
519

    
520
        int l_speed = teleop_fluid_speed + teleop_fluid_omega;
521
        int r_speed = teleop_fluid_speed - teleop_fluid_omega;
522

    
523
        teleop_l_speed = max(MIN_ABSOLUTE_SPEED,
524
                             min(MAX_ABSOLUTE_SPEED, l_speed));
525
        teleop_r_speed = max(MIN_ABSOLUTE_SPEED,
526
                             min(MAX_ABSOLUTE_SPEED, r_speed));
527
    }
528

    
529
    void SimFrame::teleop()
530
    {
531
        switch (teleop_type)
532
        {
533
            case TELEOP_OFF:
534
                return;
535
            case TELEOP_PRECISE:
536
                teleop_move_precise();
537
                break;
538
            case TELEOP_FLUID:
539
                teleop_move_fluid();
540
                break;
541
        }
542

    
543
        ::messages::set_motors msg;
544
        msg.fl_set = true;
545
        msg.fr_set = true;
546
        msg.bl_set = true;
547
        msg.br_set = true;
548
        msg.teleop_ON = true;
549

    
550
        msg.fl_speed = teleop_l_speed;
551
        msg.fr_speed = teleop_r_speed;
552
        msg.bl_speed = teleop_l_speed;
553
        msg.br_speed = teleop_r_speed;
554

    
555
        teleop_pub.publish(msg);
556
    }
557

    
558
    void SimFrame::updateScouts()
559
    {
560

    
561
        if (last_scout_update.isZero())
562
        {
563
            last_scout_update = ros::WallTime::now();
564
            return;
565
        }
566

    
567
        path_image = path_bitmap.ConvertToImage();
568
        Refresh();
569

    
570
        M_Scout::iterator it = scouts.begin();
571
        M_Scout::iterator end = scouts.end();
572

    
573
        world_state state;
574
        state.canvas_width = width_in_meters;
575
        state.canvas_height = height_in_meters;
576

    
577
        for (; it != end; ++it)
578
        {
579

    
580
            it->second->update(SIM_TIME_REFRESH_RATE,
581
                               path_dc, sonar_dc,
582
                               path_image, lines_image, walls_image,
583
                               path_dc.GetBackground().GetColour(),
584
                               sonar_dc.GetBackground().GetColour(),
585
                               state);
586
        }
587

    
588
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
589
        {
590
            ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
591
                path_dc.GetBackground().GetColour(), state);
592
        }
593

    
594
        frame_count++;
595
    }
596

    
597
    bool SimFrame::clearCallback(std_srvs::Empty::Request&,
598
                                 std_srvs::Empty::Response&)
599
    {
600
        ROS_INFO("Clearing scoutsim.");
601
        clear();
602
        return true;
603
    }
604

    
605
    bool SimFrame::resetCallback(std_srvs::Empty::Request&,
606
                                 std_srvs::Empty::Response&)
607
    {
608
        ROS_INFO("Resetting scoutsim.");
609
        scouts.clear();
610
        id_counter = 0;
611
        spawnScout("", width_in_meters / 2.0, height_in_meters / 2.0, 0);
612
        clear();
613
        return true;
614
    }
615

    
616
    void SimFrame::wirelessCallback(const ::messages::WirelessPacket::ConstPtr& msg)
617
    {
618
        wireless_receive.publish(msg);
619
    }
620
}
621

    
622
/** @} */