Revision cf115e3d
Fixed problem with serial rx. Rosserial now works.
scout_avr/src/main.cpp | ||
---|---|---|
1 | 1 |
#include "ros.h" |
2 |
#include <std_msgs/UInt16.h>
|
|
2 |
#include <std_msgs/Int16.h> |
|
3 | 3 |
|
4 | 4 |
ros::Publisher *ptest_out; |
5 | 5 |
|
6 |
void callback(const std_msgs::UInt16& msg)
|
|
6 |
void callback(const std_msgs::Int16& msg) |
|
7 | 7 |
{ |
8 | 8 |
ptest_out->publish(&msg); |
9 | 9 |
} |
... | ... | |
11 | 11 |
int main() |
12 | 12 |
{ |
13 | 13 |
ros::NodeHandle nh; |
14 |
std_msgs::UInt16 msg;
|
|
15 |
ros::Subscriber<std_msgs::UInt16> test_in("test_in", callback);
|
|
14 |
std_msgs::Int16 msg; |
|
15 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback); |
|
16 | 16 |
ros::Publisher test_out("test_out", &msg); |
17 | 17 |
|
18 | 18 |
ptest_out = &test_out; |
... | ... | |
41 | 41 |
int main() |
42 | 42 |
{ |
43 | 43 |
char time[20]; |
44 |
int data; |
|
45 |
unsigned long now, next = 0; |
|
44 | 46 |
Atmega128rfa1 avr; |
45 | 47 |
avr.init(); |
46 |
unsigned long now, next = 0; |
|
47 | 48 |
while (1) |
48 | 49 |
{ |
49 | 50 |
now = avr.time(); |
50 | 51 |
if (now > next) { |
51 |
next = now + 100; |
|
52 |
ultoa(avr.time(), time, 10); |
|
53 |
avr.write((uint8_t*) time, strlen(time)); |
|
52 |
next = now + 500; |
|
53 |
//ultoa(avr.time(), time, 10); |
|
54 |
//avr.write((uint8_t*) time, strlen(time)); |
|
55 |
data = avr.read(); |
|
56 |
if (data >= 0) |
|
57 |
{ |
|
58 |
uint8_t data8 = data; |
|
59 |
avr.write((uint8_t*) "Received ", 9); |
|
60 |
avr.write(&data8, 1); |
|
61 |
} |
|
62 |
else |
|
63 |
avr.write((uint8_t*) ".", 1); |
|
54 | 64 |
avr.write((uint8_t*) "\n\r", 1); |
55 | 65 |
} |
56 | 66 |
} |
57 | 67 |
return 0; |
58 |
}*/ |
|
59 |
|
|
68 |
} |
|
69 |
*/ |
Also available in: Unified diff