Revision 0e77683c
ID | 0e77683c65f392af332f84aa9b31eb8614290a55 |
Added built-in scoutsim teleop!
scout/scoutsim/src/scoutsim.cpp | ||
---|---|---|
134 | 134 |
EVT_MENU(ID_MAP, scoutsim::SimFrame::showMap) |
135 | 135 |
EVT_MENU(ID_LINES, scoutsim::SimFrame::showLines) |
136 | 136 |
EVT_MENU(ID_WALLS, scoutsim::SimFrame::showWalls) |
137 |
EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop) |
|
138 |
EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise) |
|
139 |
EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid) |
|
137 | 140 |
END_EVENT_TABLE() |
138 | 141 |
|
139 | 142 |
DECLARE_APP(ScoutApp); |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
135 | 135 |
wireless_send = nh.subscribe("/wireless/send", 1000, |
136 | 136 |
&SimFrame::wirelessCallback, this); |
137 | 137 |
|
138 |
// Teleop |
|
139 |
teleop_type = TELEOP_OFF; |
|
140 |
teleop_l_speed = 0; |
|
141 |
teleop_r_speed = 0; |
|
142 |
teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000); |
|
143 |
|
|
138 | 144 |
ROS_INFO("Starting scoutsim with node name %s", |
139 | 145 |
ros::this_node::getName().c_str()) ; |
140 | 146 |
|
... | ... | |
151 | 157 |
menuView->Append(ID_LINES, _("&Lines")); |
152 | 158 |
menuView->Append(ID_WALLS, _("&Walls")); |
153 | 159 |
|
160 |
wxMenu *menuTeleop = new wxMenu; |
|
161 |
menuTeleop->Append(ID_TELEOP_NONE, _("&None")); |
|
162 |
menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise")); |
|
163 |
menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid")); |
|
164 |
|
|
154 | 165 |
wxMenuBar *menuBar = new wxMenuBar; |
155 | 166 |
menuBar->Append(menuFile, _("&File")); |
156 | 167 |
menuBar->Append(menuSim, _("&Sim")); |
157 | 168 |
menuBar->Append(menuView, _("&View")); |
169 |
menuBar->Append(menuTeleop, _("&Teleop")); |
|
158 | 170 |
|
159 | 171 |
SetMenuBar(menuBar); |
160 | 172 |
|
... | ... | |
293 | 305 |
{ |
294 | 306 |
ros::spinOnce(); |
295 | 307 |
|
308 |
teleop(); |
|
309 |
|
|
296 | 310 |
updateScouts(); |
297 | 311 |
|
298 | 312 |
if (!ros::ok()) |
... | ... | |
315 | 329 |
} |
316 | 330 |
} |
317 | 331 |
|
332 |
void SimFrame::stopTeleop(wxCommandEvent& event) |
|
333 |
{ |
|
334 |
teleop_type = TELEOP_OFF; |
|
335 |
teleop_l_speed = 0; |
|
336 |
teleop_r_speed = 0; |
|
337 |
} |
|
338 |
|
|
339 |
void SimFrame::startTeleopPrecise(wxCommandEvent& event) |
|
340 |
{ |
|
341 |
teleop_type = TELEOP_PRECISE; |
|
342 |
teleop_l_speed = 0; |
|
343 |
teleop_r_speed = 0; |
|
344 |
} |
|
345 |
|
|
346 |
void SimFrame::startTeleopFluid(wxCommandEvent& event) |
|
347 |
{ |
|
348 |
teleop_type = TELEOP_FLUID; |
|
349 |
teleop_l_speed = 0; |
|
350 |
teleop_r_speed = 0; |
|
351 |
teleop_fluid_speed = 0; |
|
352 |
teleop_fluid_omega = 0; |
|
353 |
} |
|
354 |
|
|
355 |
void SimFrame::teleop_move_precise() |
|
356 |
{ |
|
357 |
// Default to stop |
|
358 |
teleop_l_speed = 0; |
|
359 |
teleop_r_speed = 0; |
|
360 |
|
|
361 |
if (wxGetKeyState(WXK_UP)) |
|
362 |
{ |
|
363 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
364 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
365 |
} |
|
366 |
else if (wxGetKeyState(WXK_DOWN)) |
|
367 |
{ |
|
368 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
369 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
370 |
} |
|
371 |
else if (wxGetKeyState(WXK_LEFT)) |
|
372 |
{ |
|
373 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
374 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
375 |
} |
|
376 |
else if (wxGetKeyState(WXK_RIGHT)) |
|
377 |
{ |
|
378 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
379 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
380 |
} |
|
381 |
} |
|
382 |
|
|
383 |
void SimFrame::teleop_move_fluid() |
|
384 |
{ |
|
385 |
if (wxGetKeyState(WXK_UP)) |
|
386 |
{ |
|
387 |
teleop_fluid_speed += 2; |
|
388 |
} |
|
389 |
else if (wxGetKeyState(WXK_DOWN)) |
|
390 |
{ |
|
391 |
teleop_fluid_speed -= 2; |
|
392 |
} |
|
393 |
else if (teleop_fluid_speed > 0) |
|
394 |
{ |
|
395 |
teleop_fluid_speed -= 1; |
|
396 |
} |
|
397 |
else if (teleop_fluid_speed < 0) |
|
398 |
{ |
|
399 |
teleop_fluid_speed += 1; |
|
400 |
} |
|
401 |
|
|
402 |
if (wxGetKeyState(WXK_LEFT)) |
|
403 |
{ |
|
404 |
teleop_fluid_omega -= 1; |
|
405 |
} |
|
406 |
else if (wxGetKeyState(WXK_RIGHT)) |
|
407 |
{ |
|
408 |
teleop_fluid_omega += 1; |
|
409 |
} |
|
410 |
else if (teleop_fluid_omega > 0) |
|
411 |
{ |
|
412 |
teleop_fluid_omega -= 1; |
|
413 |
} |
|
414 |
else if (teleop_fluid_omega < 0) |
|
415 |
{ |
|
416 |
teleop_fluid_omega += 1; |
|
417 |
} |
|
418 |
|
|
419 |
if (teleop_fluid_speed > TELEOP_FLUID_MAX_SPEED) |
|
420 |
{ |
|
421 |
teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED; |
|
422 |
} |
|
423 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2) |
|
424 |
{ |
|
425 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2; |
|
426 |
} |
|
427 |
|
|
428 |
teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
429 |
teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
430 |
} |
|
431 |
|
|
432 |
void SimFrame::teleop() |
|
433 |
{ |
|
434 |
switch (teleop_type) |
|
435 |
{ |
|
436 |
case TELEOP_OFF: |
|
437 |
return; |
|
438 |
case TELEOP_PRECISE: |
|
439 |
teleop_move_precise(); |
|
440 |
break; |
|
441 |
case TELEOP_FLUID: |
|
442 |
teleop_move_fluid(); |
|
443 |
break; |
|
444 |
} |
|
445 |
|
|
446 |
motors::set_motors msg; |
|
447 |
msg.fl_set = true; |
|
448 |
msg.fr_set = true; |
|
449 |
msg.bl_set = true; |
|
450 |
msg.br_set = true; |
|
451 |
|
|
452 |
msg.fl_speed = teleop_l_speed; |
|
453 |
msg.fr_speed = teleop_r_speed; |
|
454 |
msg.bl_speed = teleop_l_speed; |
|
455 |
msg.br_speed = teleop_r_speed; |
|
456 |
|
|
457 |
teleop_pub.publish(msg); |
|
458 |
} |
|
459 |
|
|
318 | 460 |
void SimFrame::updateScouts() |
319 | 461 |
{ |
320 | 462 |
if (last_scout_update.isZero()) |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
49 | 49 |
#include <wx/event.h> |
50 | 50 |
#include <wx/timer.h> |
51 | 51 |
#include <wx/string.h> |
52 |
#include <wx/utils.h> |
|
52 | 53 |
|
53 | 54 |
#include <ros/ros.h> |
54 | 55 |
|
55 | 56 |
#include <std_srvs/Empty.h> |
56 | 57 |
#include <scoutsim/Spawn.h> |
57 | 58 |
#include <scoutsim/Kill.h> |
59 |
#include <motors/set_motors.h> |
|
58 | 60 |
#include <map> |
59 | 61 |
|
60 | 62 |
#include "scout.h" |
... | ... | |
68 | 70 |
#define ID_MAP 4 |
69 | 71 |
#define ID_LINES 5 |
70 | 72 |
#define ID_WALLS 6 |
73 |
#define ID_TELEOP_NONE 7 |
|
74 |
#define ID_TELEOP_PRECISE 8 |
|
75 |
#define ID_TELEOP_FLUID 9 |
|
76 |
|
|
77 |
#define TELEOP_PRECISE_SPEED 30 |
|
78 |
#define TELEOP_FLUID_MAX_SPEED 80 |
|
79 |
|
|
80 |
// Teleop types |
|
81 |
#define TELEOP_OFF 0 |
|
82 |
#define TELEOP_PRECISE 1 |
|
83 |
#define TELEOP_FLUID 2 |
|
71 | 84 |
|
72 | 85 |
namespace scoutsim |
73 | 86 |
{ |
... | ... | |
86 | 99 |
void showMap(wxCommandEvent& event); |
87 | 100 |
void showLines(wxCommandEvent& event); |
88 | 101 |
void showWalls(wxCommandEvent& event); |
102 |
void stopTeleop(wxCommandEvent& event); |
|
103 |
void startTeleopPrecise(wxCommandEvent& event); |
|
104 |
void startTeleopFluid(wxCommandEvent& event); |
|
89 | 105 |
|
90 | 106 |
DECLARE_EVENT_TABLE() |
91 | 107 |
|
... | ... | |
93 | 109 |
void onUpdate(wxTimerEvent& evt); |
94 | 110 |
void onPaint(wxPaintEvent& evt); |
95 | 111 |
|
112 |
void teleop_move_precise(); |
|
113 |
void teleop_move_fluid(); |
|
114 |
void teleop(); |
|
115 |
|
|
96 | 116 |
void updateScouts(); |
97 | 117 |
void clear(); |
98 | 118 |
bool hasScout(const std::string& name); |
... | ... | |
107 | 127 |
ros::Subscriber wireless_send; |
108 | 128 |
ros::Publisher wireless_receive; |
109 | 129 |
|
130 |
// Teleop |
|
131 |
short teleop_l_speed; |
|
132 |
short teleop_r_speed; |
|
133 |
ros::Publisher teleop_pub; |
|
134 |
int teleop_type; |
|
135 |
|
|
136 |
short teleop_fluid_speed; |
|
137 |
short teleop_fluid_omega; |
|
138 |
|
|
110 | 139 |
ros::NodeHandle nh; |
111 | 140 |
wxTimer* update_timer; |
112 | 141 |
wxBitmap path_bitmap; |
... | ... | |
114 | 143 |
wxImage lines_image; |
115 | 144 |
wxImage walls_image; |
116 | 145 |
wxMemoryDC path_dc; |
117 |
wxMemoryDC sonar_dc;
|
|
146 |
wxMemoryDC sonar_dc;
|
|
118 | 147 |
|
119 | 148 |
uint64_t frame_count; |
120 | 149 |
|
Also available in: Unified diff