Revision ddfeb111
Fixing bugs with ghost scout (these were problems in odometry related
to constants in the simulator and coordinate frame and theta representation
in the simulator). Also made ghost scout translucent in order to differentiate
it.
| scout/scoutsim/src/ghost_scout.cpp | ||
|---|---|---|
| 76 | 76 |
|
| 77 | 77 |
void GhostScout::setPosition(const ::messages::ScoutPosition& msg) |
| 78 | 78 |
{
|
| 79 |
ROS_INFO("UPDATING GHOST SCOUT POSITION");
|
|
| 80 |
|
|
| 81 | 79 |
pos.x = start_x + msg.x; |
| 82 | 80 |
pos.y = start_y - msg.y; |
| 83 | 81 |
orient = msg.theta; |
| ... | ... | |
| 104 | 102 |
world_state state) |
| 105 | 103 |
{
|
| 106 | 104 |
|
| 105 |
wxImage rotated_image = scout_image.Rotate(orient - M_PI/2.0, |
|
| 106 |
wxPoint(scout_image.GetWidth() / 2, |
|
| 107 |
scout_image.GetHeight() / 2), |
|
| 108 |
false); |
|
| 109 |
|
|
| 110 |
for (int y = 0; y < rotated_image.GetHeight(); ++y) |
|
| 111 |
{
|
|
| 112 |
for (int x = 0; x < rotated_image.GetWidth(); ++x) |
|
| 113 |
{
|
|
| 114 |
if (rotated_image.GetRed(x, y) == 255 |
|
| 115 |
&& rotated_image.GetBlue(x, y) == 255 |
|
| 116 |
&& rotated_image.GetGreen(x, y) == 255) |
|
| 117 |
{
|
|
| 118 |
rotated_image.SetAlpha(x, y, 0); |
|
| 119 |
} |
|
| 120 |
else |
|
| 121 |
{
|
|
| 122 |
rotated_image.SetAlpha(x, y, 125); |
|
| 123 |
} |
|
| 124 |
} |
|
| 125 |
} |
|
| 126 |
|
|
| 127 |
scout = wxBitmap(rotated_image); |
|
| 128 |
|
|
| 107 | 129 |
geometry_msgs::Pose2D my_pose; |
| 108 | 130 |
my_pose.x = pos.x; |
| 109 | 131 |
my_pose.y = pos.y; |
| 110 | 132 |
my_pose.theta = orient; |
| 111 | 133 |
|
| 112 |
//ROS_INFO("Ghost scout is at position %f %f %f", pos.x, pos.y, orient);
|
|
| 134 |
ROS_INFO("Scout position %f %f %f", pos.x, pos.y, orient);
|
|
| 113 | 135 |
|
| 114 | 136 |
return my_pose; |
| 115 | 137 |
} |
Also available in: Unified diff