Revision 6e7f0a98
Changed scout_avr's main.cpp to use rosserial
scout_avr/src/bom.h | ||
---|---|---|
5 | 5 |
#include <stdint.h> |
6 | 6 |
} |
7 | 7 |
|
8 |
//#include <bom/bom.h>
|
|
8 |
#include <bom/bom.h> |
|
9 | 9 |
|
10 | 10 |
// constants for direction |
11 |
#define BOM_FRONT 0 //(bom::bom::FRONT)
|
|
12 |
#define BOM_BACK 1 //(bom::bom::BACK)
|
|
13 |
#define BOM_LEFT 2 //(bom::bom::LEFT)
|
|
14 |
#define BOM_RIGHT 3 //(bom::bom::RIGHT)
|
|
11 |
#define BOM_FRONT (bom::bom::FRONT) |
|
12 |
#define BOM_BACK (bom::bom::BACK) |
|
13 |
#define BOM_LEFT (bom::bom::LEFT) |
|
14 |
#define BOM_RIGHT (bom::bom::RIGHT) |
|
15 | 15 |
|
16 | 16 |
// timing, in us, for read of valid bits |
17 | 17 |
#define MIN_LOW_PW 600 |
scout_avr/src/main.cpp | ||
---|---|---|
1 |
#if 0 ///////////////////////////////////////////////
|
|
1 |
#if 1 ///////////////////////////////////////////////
|
|
2 | 2 |
|
3 | 3 |
#include "ros.h" |
4 |
#include "bom.h" |
|
5 |
|
|
4 | 6 |
#include <std_msgs/Int16.h> |
5 | 7 |
|
6 |
ros::Publisher *ptest_out;
|
|
8 |
ros::Publisher *pbom_pub;
|
|
7 | 9 |
|
8 | 10 |
void debug(const char *str) |
9 | 11 |
{ |
... | ... | |
11 | 13 |
|
12 | 14 |
void callback(const std_msgs::Int16& msg) |
13 | 15 |
{ |
14 |
ptest_out->publish(&msg); |
|
15 | 16 |
} |
16 | 17 |
|
17 | 18 |
int main() |
18 | 19 |
{ |
20 |
char i, id; |
|
19 | 21 |
ros::NodeHandle nh; |
20 |
std_msgs::Int16 msg;
|
|
22 |
bom::bom bom_msg;
|
|
21 | 23 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback); |
22 |
ros::Publisher test_out("test_out", &msg);
|
|
24 |
ros::Publisher bom_pub("bom", &bom_msg);
|
|
23 | 25 |
|
24 |
ptest_out = &test_out;
|
|
26 |
pbom_pub = &bom_pub;
|
|
25 | 27 |
|
26 |
msg.data = 0; |
|
27 | 28 |
nh.initNode(); |
28 | 29 |
nh.subscribe(test_in); |
29 |
nh.advertise(test_out);
|
|
30 |
nh.advertise(bom_pub);
|
|
30 | 31 |
|
32 |
id = 0; |
|
31 | 33 |
while (1) |
32 | 34 |
{ |
35 |
id++; |
|
36 |
if (id == 0x40) { |
|
37 |
id = 0; |
|
38 |
} |
|
39 |
set_robot_id(id); |
|
40 |
for (i = 0; i < 4; i++) { |
|
41 |
bom_send(i); |
|
42 |
int msg = bom_get(i); |
|
43 |
if (msg != BOM_NO_DATA) { |
|
44 |
bom_msg.sender = bom_msg_get_robot_id(msg); |
|
45 |
bom_msg.send_dir = bom_msg_get_dir(msg); |
|
46 |
bom_msg.recv_dir = i; |
|
47 |
bom_pub.publish(&bom_msg); |
|
48 |
} |
|
49 |
} |
|
33 | 50 |
nh.spinOnce(); |
34 | 51 |
} |
35 | 52 |
|
... | ... | |
71 | 88 |
{ |
72 | 89 |
now = avr.time(); |
73 | 90 |
if (now > next) { |
74 |
next = now + 100;
|
|
91 |
next = now + 500;
|
|
75 | 92 |
/*ultoa(now, buf, 10); |
76 | 93 |
avr.puts(buf); |
77 | 94 |
avr.puts("\r\n");*/ |
Also available in: Unified diff