Revision 93cebb99
added BomControl class and to the scout sensor
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 121 | 121 |
&Scout::query_linesensor_callback, |
| 122 | 122 |
this); |
| 123 | 123 |
|
| 124 |
query_BOM_srv = |
|
| 125 |
node.advertiseService("query_BOM",
|
|
| 126 |
&Scout::query_BOM_callback, |
|
| 127 |
this); |
|
| 128 |
|
|
| 129 |
|
|
| 124 | 130 |
for (unsigned int i = 0; i < NUM_LINESENSORS; i++) |
| 125 | 131 |
{
|
| 126 | 132 |
linesensor_readings.push_back(0); |
| 127 | 133 |
} |
| 128 | 134 |
|
| 135 |
for (unsigned int i = 0; i < NUM_BOMS; i++) |
|
| 136 |
{
|
|
| 137 |
BOM_readings.push_back(0); |
|
| 138 |
BOM_senders.push_back(0); |
|
| 139 |
} |
|
| 140 |
|
|
| 129 | 141 |
// Initialize sonar |
| 130 | 142 |
sonar_position = 0; |
| 131 | 143 |
sonar_stop_l = 0; |
| ... | ... | |
| 276 | 288 |
return true; |
| 277 | 289 |
} |
| 278 | 290 |
|
| 291 |
bool Scout::query_BOM_callback(::messages::query_boms::Request&, |
|
| 292 |
::messages::query_boms::Response& res) |
|
| 293 |
{
|
|
| 294 |
res.readings = BOM_readings; |
|
| 295 |
res.senders = BOM_senders; |
|
| 296 |
|
|
| 297 |
return true; |
|
| 298 |
} |
|
| 299 |
|
|
| 279 | 300 |
// Scale to linesensor value |
| 280 | 301 |
unsigned int Scout::rgb_to_grey(unsigned char r, |
| 281 | 302 |
unsigned char g, |
| ... | ... | |
| 429 | 450 |
} |
| 430 | 451 |
} |
| 431 | 452 |
|
| 432 |
void Scout::update_BOM(int bom_index, bool bom_value) {
|
|
| 453 |
void Scout::update_BOM(int bom_index, |
|
| 454 |
unsigned int bom_value, |
|
| 455 |
unsigned int sender) {
|
|
| 433 | 456 |
ROS_INFO("BOM%d: %d", bom_index, bom_value);
|
| 457 |
BOM_readings[bom_index] = bom_value; |
|
| 458 |
BOM_senders[bom_index] = sender; |
|
| 434 | 459 |
} |
| 435 | 460 |
|
| 436 |
|
|
| 437 | 461 |
/// Sends back the position of this scout so scoutsim can save |
| 438 | 462 |
/// the world state |
| 439 | 463 |
/// @todo remove dt param |
Also available in: Unified diff