Revision 43811241 scout/scoutsim/src/sim_frame.cpp

View differences:

scout/scoutsim/src/sim_frame.cpp
88 88
        wxBitmap lines_bitmap;
89 89
        wxBitmap walls_bitmap;
90 90
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
91
        sonar_visual_on = true;
92 91

  
93 92
        // Try to load the file; if it fails, make a new blank file
94 93
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
......
113 112
        spawn_srv = nh.advertiseService("spawn",
114 113
                                        &SimFrame::spawnCallback, this);
115 114
        kill_srv = nh.advertiseService("kill",
116
                                       &SimFrame::killCallback, this);
115
                                        &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);
117 122

  
118 123
        // Subscribe and publisher wirless from robots
119 124
        wireless_receive = nh.advertise< ::messages::WirelessPacket>(
......
122 127
            &SimFrame::wirelessCallback, this);
123 128

  
124 129
        // Teleop
125
        teleop_type = TELEOP_PRECISE;
130
        teleop_type = TELEOP_OFF;
126 131
        teleop_l_speed = 0;
127 132
        teleop_r_speed = 0;
128 133
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
......
136 141
        menuFile->Append(ID_QUIT, _("E&xit"));
137 142

  
138 143
        wxMenu *menuSim = new wxMenu;
139
        menuSim->Append(ID_SONAR, _("S&onar"));
140 144
        menuSim->Append(ID_CLEAR, _("&Clear"));
141 145

  
142 146
        wxMenu *menuView = new wxMenu;
......
149 153
        menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
150 154
        menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
151 155

  
152
        wxMenu *menuTest = new wxMenu;
153
        menuTest->Append(ID_TEST, _("&Scouts go here"));
154

  
155 156
        wxMenuBar *menuBar = new wxMenuBar;
156 157
        menuBar->Append(menuFile, _("&File"));
157 158
        menuBar->Append(menuSim, _("&Sim"));
158 159
        menuBar->Append(menuView, _("&View"));
159 160
        menuBar->Append(menuTeleop, _("&Teleop"));
160
        menuBar->Append(menuTest, _("T&est"));
161 161

  
162 162
        SetMenuBar(menuBar);
163 163

  
......
178 178
        std::string name = spawnScout(req.name, req.x, req.y, req.theta);
179 179
        if (name.empty())
180 180
        {
181
            ROS_ERROR("A scout named [%s] already exists", req.name.c_str());
181
            ROS_WARN("A scout named [%s] already exists", req.name.c_str());
182 182
            return false;
183 183
        }
184 184

  
185 185
        res.name = name;
186

  
187 186
        return true;
188 187
    }
189 188

  
......
193 192
        M_Scout::iterator it = scouts.find(req.name);
194 193
        if (it == scouts.end())
195 194
        {
196
            ROS_ERROR("Tried to kill scout [%s], which does not exist",
197
                      req.name.c_str());
195
            ROS_WARN("Tried to kill scout [%s], which does not exist",
196
                     req.name.c_str());
198 197
            return false;
199 198
        }
200 199

  
......
203 202
        return true;
204 203
    }
205 204

  
205
    bool SimFrame::setSonarVizCallback(SetSonarViz::Request& req,
206
                                       SetSonarViz::Response&)
207
    {
208
        M_Scout::iterator it = scouts.find(req.scout_name);
209
        if (it == scouts.end())
210
        {
211
            ROS_WARN("Tried to set sonar on scout [%s], which does not exist",
212
                     req.scout_name.c_str());
213
            return false;
214
        }
215

  
216
        it->second->set_sonar_visual(req.on);
217
        return true;
218
    }
219

  
220
    bool SimFrame::setGhostCallback(SetGhost::Request& req,
221
                                    SetGhost::Response&)
222
    {
223
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
224
        {
225
            if (ghost_scouts.at(i)->get_name() == req.scout_name)
226
            {
227
                ghost_scouts.at(i)->set_visible(req.on);
228
                return true;
229
            }
230
        }
231

  
232
        ROS_WARN("Tried to set ghost on scout [%s], which does not exist",
233
                 req.scout_name.c_str());
234
        return false;
235
    }
236

  
237
    bool SimFrame::setTeleopCallback(SetTeleop::Request& req,
238
                                     SetTeleop::Response&)
239
    {
240
        std::stringstream ss;
241
        ss << "/" << req.scout_name << "/set_motors";
242
        teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
243

  
244
        return true;
245
    }
246

  
206 247
    bool SimFrame::hasScout(const std::string& name)
207 248
    {
208 249
        return scouts.find(name) != scouts.end();
......
214 255
        std::string real_name = name;
215 256
        if (real_name.empty())
216 257
        {
258
            // Generate the name scoutX, where X is an increasing number.
217 259
            do
218 260
            {
219 261
                std::stringstream ss;
220 262
                ss << "scout" << ++id_counter;
221 263
                real_name = ss.str();
222
            } while (hasScout(real_name));
264
            }
265
            while (hasScout(real_name));
223 266
        }
224 267
        else
225 268
        {
......
280 323
        clear();
281 324
    }
282 325

  
283
    void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event))
284
    {
285
        sonar_visual_on = not sonar_visual_on;
286
        clear();
287
    }
288

  
289
    void SimFrame::doTest(wxCommandEvent& WXUNUSED(event))
290
    {
291
        clear();
292
    }
293

  
294 326
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
295 327
    {
296 328
        display_map_name = map_base_name;
......
353 385
        {
354 386
            it->second->paint(dc);
355 387
        }
356
        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
388
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
357 389
        {
358 390
            ghost_scouts.at(i)->paint(dc);
359 391
        }
......
410 442
        }
411 443
        else if (wxGetKeyState(WXK_LEFT))
412 444
        {
413
            teleop_l_speed = -TELEOP_PRECISE_SPEED * 4;
414
            teleop_r_speed = TELEOP_PRECISE_SPEED * 4;
445
            teleop_l_speed = -TELEOP_PRECISE_TURN_SPEED;
446
            teleop_r_speed = TELEOP_PRECISE_TURN_SPEED;
415 447
        }
416 448
        else if (wxGetKeyState(WXK_RIGHT))
417 449
        {
418
            teleop_l_speed = TELEOP_PRECISE_SPEED * 4;
419
            teleop_r_speed = -TELEOP_PRECISE_SPEED * 4;
450
            teleop_l_speed = TELEOP_PRECISE_TURN_SPEED;
451
            teleop_r_speed = -TELEOP_PRECISE_TURN_SPEED;
420 452
        }
421 453
    }
422 454

  
......
531 563
        for (; it != end; ++it)
532 564
        {
533 565
            it->second->update(SIM_TIME_REFRESH_RATE,
534
                               path_dc,sonar_dc,sonar_visual_on,
566
                               path_dc, sonar_dc,
535 567
                               path_image, lines_image, walls_image,
536 568
                               path_dc.GetBackground().GetColour(),
537 569
                               sonar_dc.GetBackground().GetColour(),
538 570
                               state);
539 571
        }
540 572

  
541
        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
573
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
542 574
        {
543 575
            ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
544 576
                path_dc.GetBackground().GetColour(), state);

Also available in: Unified diff