Revision ddfeb111
| ID | ddfeb11183e90127649a5f69425bedc926bda71c |
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/libscout/src/behaviors/Odometry.cpp | ||
|---|---|---|
| 23 | 23 |
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks; |
| 24 | 24 |
motor_br_dist = scout_enc.br_ticks - motor_br_ticks; |
| 25 | 25 |
|
| 26 |
ROS_INFO("fl %d fr %d bl %d br %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);
|
|
| 27 |
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
|
|
| 28 |
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
|
|
| 29 |
|
|
| 30 | 26 |
// Get Left and Right distance |
| 31 | 27 |
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + |
| 32 | 28 |
((float)(motor_bl_dist))/2.0); |
| ... | ... | |
| 34 | 30 |
((float)(motor_br_dist))/2.0); |
| 35 | 31 |
|
| 36 | 32 |
total_dist = (left_dist)/2.0 + (right_dist)/2.0; |
| 37 |
theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE); |
|
| 38 |
|
|
| 39 |
ROS_INFO("l: %f r: %f theta: %f", left_dist, right_dist, theta);
|
|
| 40 |
|
|
| 41 |
scout_pos->x += total_dist*sin(theta); |
|
| 42 |
scout_pos->y += total_dist*cos(theta); |
|
| 33 |
theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE); |
|
| 34 |
|
|
| 35 |
//Use negative theta because we measure theta from 0 on the |
|
| 36 |
//right to 180 on the left counter-clock-wise, but this is |
|
| 37 |
//negative theta in the coordinate frame. |
|
| 38 |
//Also, subtract the delta from y because positive y is down. |
|
| 39 |
scout_pos->x += total_dist*cos(-theta); |
|
| 40 |
scout_pos->y -= total_dist*sin(-theta); |
|
| 43 | 41 |
scout_pos->theta = fmod(theta, 2*M_PI); |
| 44 | 42 |
|
| 45 | 43 |
//Save state for next time in. |
| ... | ... | |
| 65 | 63 |
position.y = scout_pos->y; |
| 66 | 64 |
position.theta = scout_pos->theta; |
| 67 | 65 |
scout_position.publish(position); |
| 68 |
|
|
| 69 |
ROS_INFO("Scout is at x: %f y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
|
|
| 70 | 66 |
} |
| 71 | 67 |
} |
| scout/libscout/src/behaviors/Odometry.h | ||
|---|---|---|
| 6 | 6 |
#include "messages/ScoutPosition.h" |
| 7 | 7 |
|
| 8 | 8 |
|
| 9 |
#define WHEEL_RADIUS 2 |
|
| 9 |
#define WHEEL_RADIUS .2
|
|
| 10 | 10 |
#define WHEEL_CIRCUM (2*M_PI*WHEEL_RADIUS) |
| 11 |
#define WHEEL_BASE 5
|
|
| 12 |
#define ENCODER_COUNT (4*1200*M_PI/5)
|
|
| 11 |
#define WHEEL_BASE 1.2
|
|
| 12 |
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
|
|
| 13 | 13 |
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT) |
| 14 | 14 |
|
| 15 | 15 |
typedef struct{
|
| 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 |
} |
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 465 | 465 |
|
| 466 | 466 |
Pose p; |
| 467 | 467 |
p.x = pos.x; |
| 468 |
p.y = state.canvas_height - pos.y;
|
|
| 468 |
p.y = pos.y; |
|
| 469 | 469 |
p.theta = orient; |
| 470 | 470 |
p.linear_velocity = l_speed; |
| 471 | 471 |
p.angular_velocity = r_speed; |
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 122 | 122 |
&SimFrame::wirelessCallback, this); |
| 123 | 123 |
|
| 124 | 124 |
// Teleop |
| 125 |
teleop_type = TELEOP_OFF;
|
|
| 125 |
teleop_type = TELEOP_PRECISE;
|
|
| 126 | 126 |
teleop_l_speed = 0; |
| 127 | 127 |
teleop_r_speed = 0; |
| 128 | 128 |
teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
|
Also available in: Unified diff