Revision e3f69e61
ID | e3f69e61ab9e6edabd24ed28559fd8eaebc4eec2 |
Fixed up scoutsim package. Sorry.
scout/scoutsim/manifest.xml | ||
---|---|---|
15 | 15 |
<depend package="std_srvs"/> |
16 | 16 |
<depend package="motors" /> |
17 | 17 |
<depend package="encoders" /> |
18 |
<depend package="geometry_msgs" /> |
|
18 | 19 |
|
19 | 20 |
<rosdep name="wxwidgets"/> |
20 | 21 |
|
... | ... | |
36 | 37 |
<depend package="std_srvs"/> |
37 | 38 |
<depend package="motors" /> |
38 | 39 |
<depend package="encoders" /> |
40 |
<depend package="geometry_msgs" /> |
|
39 | 41 |
|
40 | 42 |
<msgs> |
41 | 43 |
msg/Color.msg msg/Pose.msg msg/Velocity.msg |
scout/scoutsim/src/scout.cpp | ||
---|---|---|
187 | 187 |
pos.y += cos(orient + PI/2.0) * lin_vel * dt; |
188 | 188 |
|
189 | 189 |
// Clamp to screen size |
190 |
if (pos.x < 0 || pos.x >= canvas_width |
|
191 |
|| pos.y < 0 || pos.y >= canvas_height) |
|
190 |
if (pos.x < 0 || pos.x >= state.canvas_width
|
|
191 |
|| pos.y < 0 || pos.y >= state.canvas_height)
|
|
192 | 192 |
{ |
193 | 193 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y); |
194 | 194 |
} |
195 | 195 |
|
196 |
pos.x = std::min(std::max(pos.x, 0.0f), canvas_width); |
|
197 |
pos.y = std::min(std::max(pos.y, 0.0f), canvas_height); |
|
196 |
pos.x = std::min(std::max(pos.x, 0.0f), state.canvas_width);
|
|
197 |
pos.y = std::min(std::max(pos.y, 0.0f), state.canvas_height);
|
|
198 | 198 |
|
199 | 199 |
int canvas_x = pos.x * meter; |
200 | 200 |
int canvas_y = pos.y * meter; |
... | ... | |
224 | 224 |
|
225 | 225 |
Pose p; |
226 | 226 |
p.x = pos.x; |
227 |
p.y = canvas_height - pos.y; |
|
227 |
p.y = state.canvas_height - pos.y;
|
|
228 | 228 |
p.theta = orient; |
229 | 229 |
p.linear_velocity = lin_vel; |
230 | 230 |
p.angular_velocity = ang_vel; |
... | ... | |
252 | 252 |
old_pos.x * meter, old_pos.y * meter); |
253 | 253 |
} |
254 | 254 |
} |
255 |
|
|
256 |
geometry_msgs::Pose2D my_pose; |
|
257 |
my_pose.x = pos.x; |
|
258 |
my_pose.y = pos.y; |
|
259 |
my_pose.theta = orient; |
|
260 |
|
|
261 |
return my_pose; |
|
255 | 262 |
} |
256 | 263 |
|
257 | 264 |
void Scout::paint(wxDC& dc) |
scout/scoutsim/src/scout.h | ||
---|---|---|
58 | 58 |
#include <scoutsim/SetPen.h> |
59 | 59 |
#include <scoutsim/Color.h> |
60 | 60 |
|
61 |
#include <geometry_msgs/Pose2D.h> |
|
62 |
|
|
61 | 63 |
#include <wx/wx.h> |
62 | 64 |
|
65 |
#include "scoutsim_internal.h" |
|
66 |
|
|
63 | 67 |
//#include "scout_constants.h" |
64 | 68 |
|
65 | 69 |
#define PI 3.14159265 |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
244 | 244 |
|
245 | 245 |
M_Scout::iterator it = scouts.begin(); |
246 | 246 |
M_Scout::iterator end = scouts.end(); |
247 |
|
|
248 |
world_state state; |
|
249 |
state.canvas_width = width_in_meters; |
|
250 |
state.canvas_height = height_in_meters; |
|
251 |
|
|
247 | 252 |
for (; it != end; ++it) |
248 | 253 |
{ |
249 | 254 |
it->second->update(0.016, path_dc, path_image, |
250 | 255 |
path_dc.GetBackground().GetColour(), |
251 |
width_in_meters, height_in_meters);
|
|
256 |
state);
|
|
252 | 257 |
} |
253 | 258 |
|
254 | 259 |
frame_count++; |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
57 | 57 |
#include <map> |
58 | 58 |
|
59 | 59 |
#include "scout.h" |
60 |
#include "scoutsim_internal.h" |
|
60 | 61 |
|
61 | 62 |
#define SCOUTSIM_NUM_SCOUTS 1 |
62 | 63 |
|
63 | 64 |
namespace scoutsim |
64 | 65 |
{ |
65 |
/** |
|
66 |
* State of the whole world, positions of all scouts! |
|
67 |
* Passed by reference to scouts for reading on update loop. |
|
68 |
* Updated only by sim_frame to prevent concurrency problems. |
|
69 |
*/ |
|
70 |
typedef struct world_state |
|
71 |
{ |
|
72 |
float canvas_width; |
|
73 |
float canvas_height; |
|
74 |
|
|
75 |
} world_state; |
|
76 |
|
|
77 | 66 |
class SimFrame : public wxFrame |
78 | 67 |
{ |
79 | 68 |
public: |
Also available in: Unified diff