Revision 43811241
Added services to set individual control for visualization tools.
However, there is a bug in the sonar viz for multiple scouts, looking into it now.
| 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