Revision 0e143737

View differences:

scout/scoutsim/CMakeLists.txt
28 28
include_directories( ${wxWidgets_INCLUDE_DIRS} )
29 29

  
30 30
rosbuild_add_boost_directories()
31
rosbuild_add_executable(scoutsim_node src/scoutsim.cpp src/scout.cpp src/ghost_scout.cpp src/sim_frame.cpp)
31
rosbuild_add_executable(scoutsim_node src/scoutsim.cpp src/scout.cpp src/ghost_scout.cpp src/sim_frame.cpp src/emitter.cpp)
32 32
rosbuild_link_boost(scoutsim_node thread)
33 33
target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})
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
/** @} */
scout/scoutsim/src/sim_frame.h
65 65
#include <map>
66 66

  
67 67
#include "scout.h"
68
#include "emitter.h"
68 69
#include "ghost_scout.h"
69 70
#include "scoutsim_internal.h"
70 71
#include "messages/WirelessPacket.h"
......
102 103

  
103 104
            std::string spawnScout(const std::string& name,
104 105
                    float x, float y, float angle);
106
            std::string spawnEmitter(const std::string& name,
107
                    float x, float y, float angle);
105 108

  
106 109
            void onQuit(wxCommandEvent& event);
107 110
            void onAbout(wxCommandEvent& event);
......
117 120

  
118 121
        private:
119 122

  
123
            typedef std::map<std::string, ScoutPtr> M_Scout;
124
            typedef std::map<std::string, EmitterPtr> M_Emitter;
125

  
120 126
            void onUpdate(wxTimerEvent& evt);
121 127
            void onPaint(wxPaintEvent& evt);
122 128

  
......
129 135
            void updateScouts();
130 136
            void clear();
131 137
            bool hasScout(const std::string& name);
138
            bool hasEmitter(const std::string& name);
132 139

  
133 140
            bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
134 141
            bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
135 142
            bool spawnCallback(Spawn::Request&, Spawn::Response&);
143
            bool spawnEmCallback(Spawn::Request&, Spawn::Response&);
136 144
            bool killCallback(Kill::Request&, Kill::Response&);
137 145

  
138 146
            bool setSonarVizCallback(SetSonarViz::Request&, SetSonarViz::Response&);
......
140 148
            bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&);
141 149

  
142 150
            void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg);
151
            void readBOM(); 
152

  
143 153

  
144 154
            ros::Subscriber wireless_send;
145 155
            ros::Publisher wireless_receive;
146 156

  
157
            //emitter
158
            float em_aperture;
159
            int em_distance;
160

  
147 161
            // Teleop
148 162
            short teleop_l_speed;
149 163
            short teleop_r_speed;
......
170 184
            ros::ServiceServer clear_srv;
171 185
            ros::ServiceServer reset_srv;
172 186
            ros::ServiceServer spawn_srv;
187
            ros::ServiceServer spawn_em_srv;
173 188
            ros::ServiceServer kill_srv;
174 189
            ros::ServiceServer set_sonar_viz_srv;
175 190
            ros::ServiceServer set_ghost_srv;
176 191
            ros::ServiceServer set_teleop_srv;
177 192

  
178
            typedef std::map<std::string, ScoutPtr> M_Scout;
179 193
            M_Scout scouts;
194
            M_Emitter emitters;
180 195
            std::vector<GhostScout*> ghost_scouts;
181 196
            uint32_t id_counter;
182 197

  

Also available in: Unified diff