Revision a8987cda scout/libscout/src/behaviors/wl_test.cpp

View differences:

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