Revision 47e26dee
ID | 47e26deedb4fe15ac5f77d29fe323441a05238a2 |
BOM testing code in main.cpp works with rosserial
scout_avr/src/Atmega128rfa1.h | ||
---|---|---|
9 | 9 |
|
10 | 10 |
#define MAX_SUBSCRIBERS 2 |
11 | 11 |
#define MAX_PUBLISHERS 2 |
12 |
#define INPUT_SIZE 128
|
|
13 |
#define OUTPUT_SIZE 128
|
|
12 |
#define INPUT_SIZE 256
|
|
13 |
#define OUTPUT_SIZE 256
|
|
14 | 14 |
|
15 | 15 |
class Atmega128rfa1 |
16 | 16 |
{ |
scout_avr/src/main.cpp | ||
---|---|---|
2 | 2 |
|
3 | 3 |
#include "ros.h" |
4 | 4 |
#include "bom.h" |
5 |
|
|
5 |
#include "range.h" |
|
6 | 6 |
#include <std_msgs/Int16.h> |
7 |
#include <util/delay.h> |
|
7 | 8 |
|
8 | 9 |
ros::Publisher *pbom_pub; |
9 | 10 |
|
... | ... | |
26 | 27 |
pbom_pub = &bom_pub; |
27 | 28 |
|
28 | 29 |
nh.initNode(); |
30 |
range_init(); |
|
31 |
bom_init(); |
|
29 | 32 |
nh.subscribe(test_in); |
30 | 33 |
nh.advertise(bom_pub); |
31 | 34 |
|
... | ... | |
37 | 40 |
id = 0; |
38 | 41 |
} |
39 | 42 |
set_robot_id(id); |
43 |
|
|
44 |
bom_send(id & 1); |
|
40 | 45 |
for (i = 0; i < 4; i++) { |
41 |
bom_send(i); |
|
42 | 46 |
int msg = bom_get(i); |
43 | 47 |
if (msg != BOM_NO_DATA) { |
44 | 48 |
bom_msg.sender = bom_msg_get_robot_id(msg); |
... | ... | |
47 | 51 |
bom_pub.publish(&bom_msg); |
48 | 52 |
} |
49 | 53 |
} |
54 |
|
|
50 | 55 |
nh.spinOnce(); |
56 |
_delay_ms(500); |
|
51 | 57 |
} |
52 | 58 |
|
53 | 59 |
return 0; |
Also available in: Unified diff