1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
#include <stdio.h>
#include <stdlib.h>
#include <lcm/lcm.h>
#include "common.h"
static lcm_t* g_lcm = NULL;
static int g_quit = 0;
static int g_lcmtest_primitives_t_count = 0;
static int g_lcmtest_primitives_list_t_count = 0;
static int g_lcmtest_node_t_count = 0;
static int g_lcmtest_multidim_array_t_count = 0;
static int g_lcmtest2_cross_package_t_count = 0;
static void
reset_counts() {
g_lcmtest_primitives_t_count = 0;
g_lcmtest_primitives_list_t_count = 0;
g_lcmtest_node_t_count = 0;
g_lcmtest_multidim_array_t_count = 0;
g_lcmtest2_cross_package_t_count = 0;
}
static void
lcmtest_primitives_t_handler(const lcm_recv_buf_t* rbuf,
const char* channel,
const lcmtest_primitives_t* msg, void* user)
{
// Reset all counts (maybe)
if (msg->i64 == 0) {
reset_counts();
}
lcmtest_primitives_t reply;
fill_lcmtest_primitives_t(g_lcmtest_primitives_t_count + 1,
&reply);
lcmtest_primitives_t_publish(g_lcm,
"test_lcmtest_primitives_t_reply", &reply);
clear_lcmtest_primitives_t(&reply);
g_lcmtest_primitives_t_count++;
}
static void
lcmtest_primitives_list_t_handler(const lcm_recv_buf_t* rbuf,
const char* channel,
const lcmtest_primitives_list_t* msg, void* user)
{
lcmtest_primitives_list_t reply;
fill_lcmtest_primitives_list_t(g_lcmtest_primitives_list_t_count + 1,
&reply);
lcmtest_primitives_list_t_publish(g_lcm,
"test_lcmtest_primitives_list_t_reply", &reply);
clear_lcmtest_primitives_list_t(&reply);
g_lcmtest_primitives_list_t_count++;
}
static void
lcmtest_node_t_handler(const lcm_recv_buf_t* rbuf,
const char* channel,
const lcmtest_node_t* msg, void* user)
{
lcmtest_node_t reply;
fill_lcmtest_node_t(g_lcmtest_node_t_count + 1,
&reply);
lcmtest_node_t_publish(g_lcm, "test_lcmtest_node_t_reply",
&reply);
clear_lcmtest_node_t(&reply);
g_lcmtest_node_t_count++;
}
static void
lcmtest_multidim_array_t_handler(const lcm_recv_buf_t* rbuf,
const char* channel,
const lcmtest_multidim_array_t* msg, void* user)
{
lcmtest_multidim_array_t reply;
fill_lcmtest_multidim_array_t(g_lcmtest_multidim_array_t_count + 1,
&reply);
lcmtest_multidim_array_t_publish(g_lcm,
"test_lcmtest_multidim_array_t_reply", &reply);
clear_lcmtest_multidim_array_t(&reply);
g_lcmtest_multidim_array_t_count++;
}
static void
lcmtest2_cross_package_t_handler(const lcm_recv_buf_t* rbuf,
const char* channel, const lcmtest2_cross_package_t* msg, void* user)
{
lcmtest2_cross_package_t reply;
fill_lcmtest2_cross_package_t(g_lcmtest2_cross_package_t_count + 1,
&reply);
lcmtest2_cross_package_t_publish(g_lcm,
"test_lcmtest2_cross_package_t_reply", &reply);
clear_lcmtest2_cross_package_t(&reply);
g_lcmtest2_cross_package_t_count++;
}
static void
echo_handler(const lcm_recv_buf_t *rbuf, const char* channel, void* user)
{
lcm_publish(g_lcm, "TEST_ECHO_REPLY", rbuf->data, rbuf->data_size);
}
static void
quit_handler() {
g_quit = 1;
}
// ============================
int main(int argc, char** argv)
{
g_lcm = lcm_create(NULL);
if(!g_lcm)
return 1;
lcm_subscribe(g_lcm, "TEST_QUIT", (lcm_msg_handler_t)&quit_handler, NULL);
lcm_subscribe(g_lcm, "TEST_ECHO", &echo_handler, NULL);
lcmtest_primitives_t_subscribe(g_lcm, "test_lcmtest_primitives_t",
&lcmtest_primitives_t_handler, NULL);
lcmtest_primitives_list_t_subscribe(g_lcm, "test_lcmtest_primitives_list_t",
&lcmtest_primitives_list_t_handler, NULL);
lcmtest_node_t_subscribe(g_lcm, "test_lcmtest_node_t",
&lcmtest_node_t_handler, NULL);
lcmtest_multidim_array_t_subscribe(g_lcm, "test_lcmtest_multidim_array_t",
&lcmtest_multidim_array_t_handler, NULL);
lcmtest2_cross_package_t_subscribe(g_lcm, "test_lcmtest2_cross_package_t",
&lcmtest2_cross_package_t_handler, NULL);
while (lcm_handle(g_lcm) == 0 && !g_quit) {}
lcm_destroy(g_lcm);
return 0;
}
|
