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