Revision e3f69e61
Fixed up scoutsim package. Sorry.
| 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) |
Also available in: Unified diff