Revision a8987cda
Fixes to WirelessReceiver and wl_test
Changed receive callback to use std::function so we can use std::bind to
pass non-static member functions. There is still a weird problem where
WirelessReceiver::dummy gets called many times for each incoming packet
in wl_test.
| scout/libscout/src/behaviors/wl_test.cpp | ||
|---|---|---|
| 25 | 25 |
|
| 26 | 26 |
#include "wl_test.h" |
| 27 | 27 |
|
| 28 |
#define DATA_SIZE 10 |
|
| 29 |
|
|
| 30 | 28 |
using namespace std; |
| 31 | 29 |
|
| 32 |
bool is_receiving = false; |
|
| 33 |
uint8_t highest_received = 0; |
|
| 34 |
|
|
| 35 |
void data_callback(std::vector<uint8_t> data) |
|
| 30 |
void wl_test::data_callback(std::vector<uint8_t> data) |
|
| 36 | 31 |
{
|
| 37 |
cout << "Wireless callback triggered." << endl;;
|
|
| 38 |
for (int i = 0; i < data.size(); i++)
|
|
| 39 |
{
|
|
| 40 |
if (data[i] > highest_received) |
|
| 41 |
{
|
|
| 42 |
highest_received = data[i];
|
|
| 43 |
}
|
|
| 44 |
}
|
|
| 32 |
// We got a response! Stop publishing data = [1]
|
|
| 33 |
if (no_response)
|
|
| 34 |
no_response = false;
|
|
| 35 |
|
|
| 36 |
ROS_INFO("Wireless callback triggered.");
|
|
| 37 |
data[0]++;
|
|
| 38 |
sleep(1);
|
|
| 39 |
wl_sender->send(data,1);
|
|
| 45 | 40 |
} |
| 46 | 41 |
|
| 47 | 42 |
void wl_test::run() |
| 48 | 43 |
{
|
| 49 |
motors->set_sides(20, 50, MOTOR_ABSOLUTE); |
|
| 50 |
wl_receiver->register_callback(data_callback); |
|
| 51 |
|
|
| 44 |
wl_receiver->register_callback(std::bind(&wl_test::data_callback, this, |
|
| 45 |
std::placeholders::_1)); |
|
| 52 | 46 |
while(ok()) |
| 53 | 47 |
{
|
| 54 |
if (!is_receiving) |
|
| 48 |
// Publish 1 if we are scout1 until we get a response. |
|
| 49 |
if (scout_name == "scout1" && no_response) |
|
| 55 | 50 |
{
|
| 56 |
// Fill up a bogus data array |
|
| 57 |
vector<uint8_t> data; |
|
| 58 |
for (int i = 0; i < DATA_SIZE; i++) |
|
| 59 |
{
|
|
| 60 |
data.push_back(highest_received + i); |
|
| 61 |
} |
|
| 62 |
|
|
| 63 |
wl_sender->send(data); |
|
| 64 |
cout << "Sent wireless packet." << endl; |
|
| 65 |
is_receiving = true; |
|
| 51 |
uint8_t data[1]; |
|
| 52 |
data[0] = 1; |
|
| 53 |
wl_sender->send(data,1); |
|
| 66 | 54 |
} |
| 67 |
|
|
| 68 | 55 |
spinOnce(); |
| 69 | 56 |
loop_rate->sleep(); |
| 70 | 57 |
} |
| 71 | 58 |
} |
| 72 |
|
|
Also available in: Unified diff