Revision 43811241
ID | 43811241b4b22dba6e0abbbbb16af2e6ef041c0d |
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.h | ||
---|---|---|
58 | 58 |
#include <std_srvs/Empty.h> |
59 | 59 |
#include <scoutsim/Spawn.h> |
60 | 60 |
#include <scoutsim/Kill.h> |
61 |
#include <scoutsim/SetSonarViz.h> |
|
62 |
#include <scoutsim/SetGhost.h> |
|
63 |
#include <scoutsim/SetTeleop.h> |
|
61 | 64 |
#include <motors/set_motors.h> |
62 | 65 |
#include <map> |
63 | 66 |
|
... | ... | |
76 | 79 |
#define ID_TELEOP_NONE 7 |
77 | 80 |
#define ID_TELEOP_PRECISE 8 |
78 | 81 |
#define ID_TELEOP_FLUID 9 |
79 |
#define ID_SONAR 10 |
|
80 |
#define ID_TEST 11 |
|
81 | 82 |
|
82 | 83 |
// Absolute speeds (-128 - 127) |
83 |
#define TELEOP_PRECISE_SPEED 50 |
|
84 |
/// @todo: Clean this up a little; we should be risking overflowing shorts. |
|
85 |
#define TELEOP_PRECISE_SPEED 60 |
|
86 |
#define TELEOP_PRECISE_TURN_SPEED 124 |
|
84 | 87 |
#define TELEOP_FLUID_MAX_SPEED 100 |
85 | 88 |
#define TELEOP_FLUID_INC 8 |
86 | 89 |
|
... | ... | |
109 | 112 |
void stopTeleop(wxCommandEvent& event); |
110 | 113 |
void startTeleopPrecise(wxCommandEvent& event); |
111 | 114 |
void startTeleopFluid(wxCommandEvent& event); |
112 |
void showSonar(wxCommandEvent& event); |
|
113 |
void doTest(wxCommandEvent& event); |
|
114 | 115 |
|
115 | 116 |
DECLARE_EVENT_TABLE() |
116 | 117 |
|
... | ... | |
134 | 135 |
bool spawnCallback(Spawn::Request&, Spawn::Response&); |
135 | 136 |
bool killCallback(Kill::Request&, Kill::Response&); |
136 | 137 |
|
138 |
bool setSonarVizCallback(SetSonarViz::Request&, SetSonarViz::Response&); |
|
139 |
bool setGhostCallback(SetGhost::Request&, SetGhost::Response&); |
|
140 |
bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&); |
|
141 |
|
|
137 | 142 |
void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg); |
138 | 143 |
|
139 | 144 |
ros::Subscriber wireless_send; |
... | ... | |
165 | 170 |
ros::ServiceServer reset_srv; |
166 | 171 |
ros::ServiceServer spawn_srv; |
167 | 172 |
ros::ServiceServer kill_srv; |
173 |
ros::ServiceServer set_sonar_viz_srv; |
|
174 |
ros::ServiceServer set_ghost_srv; |
|
175 |
ros::ServiceServer set_teleop_srv; |
|
168 | 176 |
|
169 | 177 |
typedef std::map<std::string, ScoutPtr> M_Scout; |
170 | 178 |
M_Scout scouts; |
... | ... | |
176 | 184 |
float width_in_meters; |
177 | 185 |
float height_in_meters; |
178 | 186 |
|
179 |
bool sonar_visual_on; |
|
180 |
|
|
181 | 187 |
std::string map_base_name; |
182 | 188 |
std::string map_lines_name; |
183 | 189 |
std::string map_walls_name; |
Also available in: Unified diff