Revision a8987cda
ID | a8987cda5e2c606b2aba534af5ee0edb027c69b3 |
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