Revision ade1b7f9
| ID | ade1b7f9e6aee15cde7890ca6330f9cd57e118b5 |
Basic line sensor readings!
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 96 | 96 |
&Scout::query_linesensor_callback, |
| 97 | 97 |
this); |
| 98 | 98 |
|
| 99 |
for (unsigned int i = 0; i < NUM_LINESENSORS; i++) |
|
| 100 |
{
|
|
| 101 |
linesensor_readings.push_back(0); |
|
| 102 |
} |
|
| 103 |
|
|
| 99 | 104 |
meter = scout.GetHeight(); |
| 100 | 105 |
} |
| 101 | 106 |
|
| ... | ... | |
| 166 | 171 |
bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&, |
| 167 | 172 |
linesensor::query_linesensor::Response& res) |
| 168 | 173 |
{
|
| 169 |
vector<unsigned int> readings; |
|
| 174 |
res.readings = linesensor_readings; |
|
| 175 |
|
|
| 176 |
return true; |
|
| 177 |
} |
|
| 170 | 178 |
|
| 171 |
for (unsigned int i = 0; i < 8; i++) |
|
| 179 |
// Scale to linesensor value |
|
| 180 |
unsigned int Scout::rgb_to_linesensor_val(unsigned char r, |
|
| 181 |
unsigned char g, |
|
| 182 |
unsigned char b) |
|
| 183 |
{
|
|
| 184 |
// Should be 0 to 255 |
|
| 185 |
unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3; |
|
| 186 |
|
|
| 187 |
/// @todo Convert to the proper range |
|
| 188 |
return 255 - grey; |
|
| 189 |
} |
|
| 190 |
|
|
| 191 |
void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta) |
|
| 192 |
{
|
|
| 193 |
linesensor_readings.clear(); |
|
| 194 |
|
|
| 195 |
double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1); |
|
| 196 |
for (int s = 0; s < NUM_LINESENSORS; s++) |
|
| 172 | 197 |
{
|
| 173 |
readings.push_back(8 - i); |
|
| 174 |
} |
|
| 198 |
double offset = -(scout_image.GetWidth() / 2) + s * spacing; |
|
| 175 | 199 |
|
| 176 |
res.readings = readings; |
|
| 200 |
int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta)); |
|
| 201 |
int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta)); |
|
| 177 | 202 |
|
| 178 |
return true; |
|
| 203 |
/* |
|
| 204 |
if (s == 0) |
|
| 205 |
{
|
|
| 206 |
cout << "s: " << s << ", offset: " << offset << ", scout: (" << x <<
|
|
| 207 |
", " << y << "), sensor: (" << sensor_x <<
|
|
| 208 |
", " << sensor_y << ")" << endl; |
|
| 209 |
} |
|
| 210 |
*/ |
|
| 211 |
|
|
| 212 |
unsigned char r = lines_image.GetRed(sensor_x, sensor_y); |
|
| 213 |
unsigned char g = lines_image.GetGreen(sensor_x, sensor_y); |
|
| 214 |
unsigned char b = lines_image.GetBlue(sensor_x, sensor_y); |
|
| 215 |
|
|
| 216 |
unsigned int reading = rgb_to_linesensor_val(r, g, b); |
|
| 217 |
|
|
| 218 |
linesensor_readings.push_back(reading); |
|
| 219 |
} |
|
| 179 | 220 |
} |
| 180 | 221 |
|
| 181 | 222 |
/// Sends back the position of this scout so scoutsim can save |
| 182 | 223 |
/// the world state |
| 183 | 224 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| 184 | 225 |
const wxImage& path_image, |
| 226 |
const wxImage& lines_image, |
|
| 185 | 227 |
wxColour background_color, |
| 186 | 228 |
world_state state) |
| 187 | 229 |
{
|
| ... | ... | |
| 225 | 267 |
wxImage rotated_image = |
| 226 | 268 |
scout_image.Rotate(orient - PI/2.0, |
| 227 | 269 |
wxPoint(scout_image.GetWidth() / 2, |
| 228 |
scout_image.GetHeight() / 2),
|
|
| 270 |
scout_image.GetHeight() / 2), |
|
| 229 | 271 |
false); |
| 230 | 272 |
|
| 231 | 273 |
for (int y = 0; y < rotated_image.GetHeight(); ++y) |
| ... | ... | |
| 252 | 294 |
p.angular_velocity = ang_vel; |
| 253 | 295 |
pose_pub.publish(p); |
| 254 | 296 |
|
| 297 |
update_linesensor(lines_image, canvas_x, canvas_y, p.theta); |
|
| 298 |
|
|
| 255 | 299 |
// Figure out (and publish) the color underneath the scout |
| 256 | 300 |
{
|
| 257 | 301 |
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); |
Also available in: Unified diff