Revision 0e143737
Created Emitter class based on Scout class
Added command 'spawn_em' to spawn emitters
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