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

View differences:

scout/scoutsim/src/sim_frame.cpp
119 119
                                        &SimFrame::resetCallback, this);
120 120
        spawn_srv = nh.advertiseService("spawn",
121 121
                                        &SimFrame::spawnCallback, this);
122
        spawn_em_srv = nh.advertiseService("spawn_em",
123
                                        &SimFrame::spawnEmCallback, this);
122 124
        kill_srv = nh.advertiseService("kill",
123 125
                                        &SimFrame::killCallback, this);
124 126
        set_sonar_viz_srv = nh.advertiseService("set_sonar_viz",
......
134 136
        wireless_send = nh.subscribe("/wireless/send", 1000,
135 137
            &SimFrame::wirelessCallback, this);
136 138

  
139
        //Emitter default values
140
        em_aperture = PI / 6.0;
141
        em_distance = 2;
142

  
137 143
        // Teleop
138 144
        teleop_type = TELEOP_OFF;
139 145
        teleop_l_speed = 0;
......
196 202
        return true;
197 203
    }
198 204

  
205

  
206
    bool SimFrame::spawnEmCallback(scoutsim::Spawn::Request  &req,
207
                                 scoutsim::Spawn::Response &res)
208
    {
209
        std::string name = spawnEmitter(req.name, req.x, req.y, req.theta);
210
        if (name.empty())
211
        {
212
            ROS_WARN("An emitter named [%s] already exists", req.name.c_str());
213
            return false;
214
        }
215

  
216
        res.name = name;
217
        return true;
218
    }
219

  
199 220
    bool SimFrame::killCallback(scoutsim::Kill::Request& req,
200 221
                                scoutsim::Kill::Response&)
201 222
    {
......
261 282
        return scouts.find(name) != scouts.end();
262 283
    }
263 284

  
285
    bool SimFrame::hasEmitter(const std::string& name)
286
    {
287
        return emitters.find(name) != emitters.end();
288
    }
289

  
264 290
    std::string SimFrame::spawnScout(const std::string& name,
265 291
                                     float x, float y, float angle)
266 292
    {
......
315 341
        return real_name;
316 342
    }
317 343

  
344

  
345
    std::string SimFrame::spawnEmitter(const std::string& name,
346
                                     float x, float y, float angle)
347
    {
348
        std::string real_name = name;
349
        if (real_name.empty())
350
        {
351
            // Generate the name emitterX, where X is an increasing number.
352
            do
353
            {
354
                std::stringstream ss;
355
                ss << "emitter" << ++id_counter;
356
                real_name = ss.str();
357
            }
358
            while (hasEmitter(real_name));
359
        }
360
        else
361
        {
362
            if (hasEmitter(real_name))
363
            {
364
                return "";
365
            }
366
        }
367

  
368
        wxImage emitter_image;
369

  
370
        // Try to load a name-specific image; if not, load the default emitter
371
        string specific_name = images_path + name + ".png";
372
        if (fileExists(specific_name))
373
        {
374
            emitter_image.LoadFile(wxString::FromAscii(specific_name.c_str()));
375
            emitter_image.SetMask(true);
376
            emitter_image.SetMaskColour(255, 255, 255);
377
        }
378
        else
379
        {
380
            ROS_INFO("LOADED EMITTER IMAGE");
381
            emitter_image.LoadFile(
382
                wxString::FromAscii((images_path + "emitter.png").c_str()));
383
            emitter_image.SetMask(true);
384
            emitter_image.SetMaskColour(255, 255, 255);
385
        }
386

  
387
        EmitterPtr t(new Emitter(ros::NodeHandle(real_name),
388
                   emitter_image, Vector2(x, y), &path_bitmap, 
389
                   angle, em_aperture, em_distance));
390
        emitters[real_name] = t;
391

  
392
        ROS_INFO("Spawning emitter [%s] at x=[%f], y=[%f], theta=[%f]",
393
                 real_name.c_str(), x, y, angle);
394

  
395
        return real_name;
396
    }
397

  
398

  
318 399
    void SimFrame::onQuit(wxCommandEvent& WXUNUSED(event))
319 400
    {
320 401
        Close(true);
......
397 478
        {
398 479
            it->second->paint(dc);
399 480
        }
481

  
482

  
483
        M_Emitter::iterator m_it = emitters.begin();
484
        M_Emitter::iterator m_end = emitters.end();
485
        for (; m_it != m_end; ++m_it)
486
        {
487
            m_it->second->paint(dc);
488
        }
489

  
400 490
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
401 491
        {
402 492
            ghost_scouts.at(i)->paint(dc);
......
570 660
        M_Scout::iterator it = scouts.begin();
571 661
        M_Scout::iterator end = scouts.end();
572 662

  
663
        M_Emitter::iterator m_it = emitters.begin();
664
        M_Emitter::iterator m_end = emitters.end();
665

  
573 666
        world_state state;
574 667
        state.canvas_width = width_in_meters;
575 668
        state.canvas_height = height_in_meters;
......
585 678
                               state);
586 679
        }
587 680

  
681

  
682
        for (; m_it != m_end; ++m_it)
683
        {
684

  
685
            m_it->second->update(SIM_TIME_REFRESH_RATE,
686
                               path_dc,
687
                               path_image, lines_image, walls_image,
688
                               path_dc.GetBackground().GetColour(),
689
                               state);
690
        }
691

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

  
698

  
594 699
        frame_count++;
595 700
    }
596 701

  
702

  
597 703
    bool SimFrame::clearCallback(std_srvs::Empty::Request&,
598 704
                                 std_srvs::Empty::Response&)
599 705
    {
......
617 723
    {
618 724
        wireless_receive.publish(msg);
619 725
    }
726

  
727

  
728
    void SimFrame::readBOM()
729
    {
730

  
731
    }
732

  
620 733
}
621 734

  
622 735
/** @} */

Also available in: Unified diff