aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorWe-unite <3205135446@qq.com>2024-06-13 18:19:08 +0800
committerWe-unite <3205135446@qq.com>2024-06-15 14:26:58 +0800
commit1fb859960b3e9c17a78754c6321da85f6b7b01b6 (patch)
treea0ac5866cc6a1677bc73d2f033a877172920cece
parent7eba07d667cf1a7613fed1170e884a3a43123fe1 (diff)
downloadWheelCtrl-1fb859960b3e9c17a78754c6321da85f6b7b01b6.tar.gz
WheelCtrl-1fb859960b3e9c17a78754c6321da85f6b7b01b6.zip
add ROBOT_CONTROL, divide udp2lcm and serial
Diffstat
-rw-r--r--CMakeLists.txt25
-rw-r--r--lcmtype/eventlog.h150
-rw-r--r--lcmtype/grid_map_t.cpp417
-rw-r--r--lcmtype/grid_map_t.h157
-rw-r--r--lcmtype/laser_t.cpp364
-rw-r--r--lcmtype/laser_t.h67
-rw-r--r--lcmtype/lcm.h316
-rw-r--r--lcmtype/lcm_coretypes.h460
-rw-r--r--lcmtype/lcmtype.h14
-rw-r--r--lcmtype/lcmtype.mk22
-rw-r--r--lcmtype/path_ctrl.lcm (renamed from path/path_ctrl.lcm)0
-rw-r--r--lcmtype/path_ctrl_t.c (renamed from path/path_ctrl_t.c)0
-rw-r--r--lcmtype/path_ctrl_t.h (renamed from path/path_ctrl_t.h)3
-rw-r--r--lcmtype/path_t.cpp297
-rw-r--r--lcmtype/path_t.h60
-rw-r--r--lcmtype/pose_t.cpp325
-rw-r--r--lcmtype/pose_t.h63
-rw-r--r--lcmtype/robot_control_t.cpp478
-rw-r--r--lcmtype/robot_control_t.h76
-rw-r--r--lib/aarch64/liblcm.sobin0 -> 95736 bytes
-rwxr-xr-xlib/x86/liblcm.so (renamed from lib/liblcm.so)bin144320 -> 144320 bytes
-rw-r--r--serial/CMakeLists.txt7
-rw-r--r--serial/serial.c (renamed from serial.c)2
-rw-r--r--udp2lcm.c198
-rw-r--r--udp2lcm/CMakeLists.txt10
-rw-r--r--udp2lcm/robotSet.c60
-rw-r--r--udp2lcm/udp.c127
-rw-r--r--udp2lcm/udp2lcm.c118
-rw-r--r--udp2lcm/udp2lcm.h45
29 files changed, 3648 insertions, 213 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt
index da5fc52..c487036 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,17 +1,18 @@
1cmake_minimum_required(VERSION 3.10) 1cmake_minimum_required(VERSION 3.10)
2project(WheelCtrl) 2project(WheelCtrl)
3 3
4# Set the output directory for the build executables. 4# 设置编译参数
5set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
6add_compile_options(-g -w) 5add_compile_options(-g -w)
6# 添加头文件路径
7include_directories(
8 ${CMAKE_SOURCE_DIR}/lcmtype
9)
10# 添加库文件路径
11link_directories(
12 ${CMAKE_SOURCE_DIR}/lib
13)
14# 设置输出路径
15set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
7 16
8# Find the required libraries 17add_subdirectory(serial)
9find_library(LCM_LIBRARY lcm lib) 18add_subdirectory(udp2lcm) \ No newline at end of file
10
11# Add the executable serial
12add_executable(serial serial.c path/path_ctrl_t.c)
13target_link_libraries(serial ${LCM_LIBRARY})
14
15# Add the executable udp2lcm
16add_executable(udp2lcm udp2lcm.c path/path_ctrl_t.c)
17target_link_libraries(udp2lcm ${LCM_LIBRARY}) \ No newline at end of file
diff --git a/lcmtype/eventlog.h b/lcmtype/eventlog.h
new file mode 100644
index 0000000..8999b1c
--- /dev/null
+++ b/lcmtype/eventlog.h
@@ -0,0 +1,150 @@
1#ifndef _LCM_EVENTLOG_H_
2#define _LCM_EVENTLOG_H_
3
4#include <stdint.h>
5#include <stdio.h>
6
7#ifdef __cplusplus
8extern "C" {
9#endif
10
11#ifndef LCM_API_FUNCTION
12#ifdef WIN32
13#define LCM_API_FUNCTION __declspec(dllexport)
14#else
15#define LCM_API_FUNCTION
16#endif // WIN32
17#endif // LCM_API_FUNCTION
18
19/**
20 * @defgroup LcmC_lcm_eventlog_t lcm_eventlog_t
21 * @ingroup LcmC
22 * @brief Read and write %LCM log files
23 *
24 * <tt> #include <lcm.h> </tt>
25 *
26 * Linking: <tt> `pkg-config --libs lcm` </tt>
27 *
28 * @{
29 */
30
31typedef struct _lcm_eventlog_t lcm_eventlog_t;
32struct _lcm_eventlog_t {
33 /**
34 * The underlying file handle. Use this at your own risk. Example use
35 * cases include implementing your own seek routine for read-mode logs, or
36 * rewinding the file pointer to the beginning of the log file.
37 */
38 FILE *f;
39 /**
40 * Do not use.
41 */
42 int64_t eventcount;
43};
44
45/**
46 * Represents a single event (message) in a log file.
47 */
48typedef struct _lcm_eventlog_event_t lcm_eventlog_event_t;
49struct _lcm_eventlog_event_t {
50 /**
51 * A monotonically increasing number assigned to the message to identify it
52 * in the log file.
53 */
54 int64_t eventnum;
55 /**
56 * Time that the message was received, in microseconds since the UNIX
57 * epoch
58 */
59 int64_t timestamp;
60 /**
61 * Length of @c channel, in bytes
62 */
63 int32_t channellen;
64 /**
65 * Length of @c data, in bytes
66 */
67 int32_t datalen;
68
69 /**
70 * Channel that the message was received on
71 */
72 char *channel;
73 /**
74 * Raw byte buffer containing the message payload.
75 */
76 void *data;
77};
78
79/**
80 * Open a log file for reading or writing.
81 *
82 * @param path Log file to open
83 * @param mode "r" (read mode) or "w" (write mode)
84 *
85 * @return a newly allocated lcm_eventlog_t, or NULL on failure.
86 */
87LCM_API_FUNCTION
88lcm_eventlog_t *lcm_eventlog_create(const char *path, const char *mode);
89
90/**
91 * Read the next event in the log file. Valid in read mode only. Free the
92 * returned structure with lcm_eventlog_free_event() after use.
93 *
94 * @param eventlog The log file object
95 *
96 * @return the next event in the log file, or NULL when the end of the file has
97 * been reached.
98 */
99LCM_API_FUNCTION
100lcm_eventlog_event_t *lcm_eventlog_read_next_event(lcm_eventlog_t *eventlog);
101
102/**
103 * Free a structure returned by lcm_eventlog_read_next_event().
104 *
105 * @param event A structure returned by lcm_eventlog_read_next_event()
106 */
107LCM_API_FUNCTION
108void lcm_eventlog_free_event(lcm_eventlog_event_t *event);
109
110/**
111 * Seek (approximately) to a particular timestamp.
112 *
113 * @param eventlog The log file object
114 * @param ts Timestamp of the target event in the log file.
115 *
116 * @return 0 on success, -1 on failure
117 */
118LCM_API_FUNCTION
119int lcm_eventlog_seek_to_timestamp(lcm_eventlog_t *eventlog, int64_t ts);
120
121/**
122 * Write an event into a log file. Valid in write mode only.
123 *
124 * @param eventlog The log file object
125 * @param event The event to write to the file. On return, the eventnum field
126 * will be filled in for you.
127 *
128 * @return 0 on success, -1 on failure.
129 */
130LCM_API_FUNCTION
131int lcm_eventlog_write_event(lcm_eventlog_t *eventlog,
132 lcm_eventlog_event_t *event);
133
134/**
135 * Close a log file and release allocated resources.
136 *
137 * @param eventlog The log file object
138 */
139LCM_API_FUNCTION
140void lcm_eventlog_destroy(lcm_eventlog_t *eventlog);
141
142/**
143 * @}
144 */
145
146#ifdef __cplusplus
147}
148#endif
149
150#endif
diff --git a/lcmtype/grid_map_t.cpp b/lcmtype/grid_map_t.cpp
new file mode 100644
index 0000000..870fd4e
--- /dev/null
+++ b/lcmtype/grid_map_t.cpp
@@ -0,0 +1,417 @@
1// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2// BY HAND!!
3//
4// Generated by lcm-gen
5
6#include "grid_map_t.h"
7#include <string.h>
8
9static int __grid_map_t_hash_computed;
10static uint64_t __grid_map_t_hash;
11
12uint64_t __grid_map_t_hash_recursive(const __lcm_hash_ptr *p) {
13 const __lcm_hash_ptr *fp;
14 for (fp = p; fp != NULL; fp = fp->parent)
15 if (fp->v == __grid_map_t_get_hash)
16 return 0;
17
18 __lcm_hash_ptr cp;
19 cp.parent = p;
20 cp.v = (void *)__grid_map_t_get_hash;
21 (void)cp;
22
23 uint64_t hash =
24 (uint64_t)0x3b95cdd95c1cd816LL + __int64_t_hash_recursive(&cp) +
25 __int8_t_hash_recursive(&cp) + __double_hash_recursive(&cp) +
26 __double_hash_recursive(&cp) + __double_hash_recursive(&cp) +
27 __int32_t_hash_recursive(&cp) + __int32_t_hash_recursive(&cp) +
28 __int32_t_hash_recursive(&cp) + __string_hash_recursive(&cp) +
29 __byte_hash_recursive(&cp);
30
31 return (hash << 1) + ((hash >> 63) & 1);
32}
33
34int64_t __grid_map_t_get_hash(void) {
35 if (!__grid_map_t_hash_computed) {
36 __grid_map_t_hash = (int64_t)__grid_map_t_hash_recursive(NULL);
37 __grid_map_t_hash_computed = 1;
38 }
39
40 return __grid_map_t_hash;
41}
42
43int __grid_map_t_encode_array(void *buf, int offset, int maxlen,
44 const grid_map_t *p, int elements) {
45 int pos = 0, element;
46 int thislen;
47
48 for (element = 0; element < elements; element++) {
49
50 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
51 &(p[element].utime), 1);
52 if (thislen < 0)
53 return thislen;
54 else
55 pos += thislen;
56
57 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
58 &(p[element].encoding), 1);
59 if (thislen < 0)
60 return thislen;
61 else
62 pos += thislen;
63
64 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
65 &(p[element].x0), 1);
66 if (thislen < 0)
67 return thislen;
68 else
69 pos += thislen;
70
71 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
72 &(p[element].y0), 1);
73 if (thislen < 0)
74 return thislen;
75 else
76 pos += thislen;
77
78 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
79 &(p[element].meters_per_pixel), 1);
80 if (thislen < 0)
81 return thislen;
82 else
83 pos += thislen;
84
85 thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos,
86 &(p[element].width), 1);
87 if (thislen < 0)
88 return thislen;
89 else
90 pos += thislen;
91
92 thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos,
93 &(p[element].height), 1);
94 if (thislen < 0)
95 return thislen;
96 else
97 pos += thislen;
98
99 thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos,
100 &(p[element].datalen), 1);
101 if (thislen < 0)
102 return thislen;
103 else
104 pos += thislen;
105
106 thislen = __string_encode_array(buf, offset + pos, maxlen - pos,
107 &(p[element].src), 1);
108 if (thislen < 0)
109 return thislen;
110 else
111 pos += thislen;
112
113 thislen = __byte_encode_array(buf, offset + pos, maxlen - pos,
114 p[element].dst, p[element].datalen);
115 if (thislen < 0)
116 return thislen;
117 else
118 pos += thislen;
119 }
120 return pos;
121}
122
123int grid_map_t_encode(void *buf, int offset, int maxlen, const grid_map_t *p) {
124 int pos = 0, thislen;
125 int64_t hash = __grid_map_t_get_hash();
126
127 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
128 if (thislen < 0)
129 return thislen;
130 else
131 pos += thislen;
132
133 thislen = __grid_map_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
134 if (thislen < 0)
135 return thislen;
136 else
137 pos += thislen;
138
139 return pos;
140}
141
142int __grid_map_t_encoded_array_size(const grid_map_t *p, int elements) {
143 int size = 0, element;
144 for (element = 0; element < elements; element++) {
145
146 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
147
148 size += __int8_t_encoded_array_size(&(p[element].encoding), 1);
149
150 size += __double_encoded_array_size(&(p[element].x0), 1);
151
152 size += __double_encoded_array_size(&(p[element].y0), 1);
153
154 size += __double_encoded_array_size(&(p[element].meters_per_pixel), 1);
155
156 size += __int32_t_encoded_array_size(&(p[element].width), 1);
157
158 size += __int32_t_encoded_array_size(&(p[element].height), 1);
159
160 size += __int32_t_encoded_array_size(&(p[element].datalen), 1);
161
162 size += __string_encoded_array_size(&(p[element].src), 1);
163
164 size += __byte_encoded_array_size(p[element].dst, p[element].datalen);
165 }
166 return size;
167}
168
169int grid_map_t_encoded_size(const grid_map_t *p) {
170 return 8 + __grid_map_t_encoded_array_size(p, 1);
171}
172
173int __grid_map_t_decode_array(const void *buf, int offset, int maxlen,
174 grid_map_t *p, int elements) {
175 int pos = 0, thislen, element;
176
177 for (element = 0; element < elements; element++) {
178
179 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
180 &(p[element].utime), 1);
181 if (thislen < 0)
182 return thislen;
183 else
184 pos += thislen;
185
186 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
187 &(p[element].encoding), 1);
188 if (thislen < 0)
189 return thislen;
190 else
191 pos += thislen;
192
193 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
194 &(p[element].x0), 1);
195 if (thislen < 0)
196 return thislen;
197 else
198 pos += thislen;
199
200 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
201 &(p[element].y0), 1);
202 if (thislen < 0)
203 return thislen;
204 else
205 pos += thislen;
206
207 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
208 &(p[element].meters_per_pixel), 1);
209 if (thislen < 0)
210 return thislen;
211 else
212 pos += thislen;
213
214 thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos,
215 &(p[element].width), 1);
216 if (thislen < 0)
217 return thislen;
218 else
219 pos += thislen;
220
221 thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos,
222 &(p[element].height), 1);
223 if (thislen < 0)
224 return thislen;
225 else
226 pos += thislen;
227
228 thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos,
229 &(p[element].datalen), 1);
230 if (thislen < 0)
231 return thislen;
232 else
233 pos += thislen;
234
235 thislen = __string_decode_array(buf, offset + pos, maxlen - pos,
236 &(p[element].src), 1);
237 if (thislen < 0)
238 return thislen;
239 else
240 pos += thislen;
241
242 p[element].dst =
243 (uint8_t *)lcm_malloc(sizeof(uint8_t) * p[element].datalen);
244 thislen = __byte_decode_array(buf, offset + pos, maxlen - pos,
245 p[element].dst, p[element].datalen);
246 if (thislen < 0)
247 return thislen;
248 else
249 pos += thislen;
250 }
251 return pos;
252}
253
254int __grid_map_t_decode_array_cleanup(grid_map_t *p, int elements) {
255 int element;
256 for (element = 0; element < elements; element++) {
257
258 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
259
260 __int8_t_decode_array_cleanup(&(p[element].encoding), 1);
261
262 __double_decode_array_cleanup(&(p[element].x0), 1);
263
264 __double_decode_array_cleanup(&(p[element].y0), 1);
265
266 __double_decode_array_cleanup(&(p[element].meters_per_pixel), 1);
267
268 __int32_t_decode_array_cleanup(&(p[element].width), 1);
269
270 __int32_t_decode_array_cleanup(&(p[element].height), 1);
271
272 __int32_t_decode_array_cleanup(&(p[element].datalen), 1);
273
274 __string_decode_array_cleanup(&(p[element].src), 1);
275
276 __byte_decode_array_cleanup(p[element].dst, p[element].datalen);
277 if (p[element].dst)
278 free(p[element].dst);
279 }
280 return 0;
281}
282
283int grid_map_t_decode(const void *buf, int offset, int maxlen, grid_map_t *p) {
284 int pos = 0, thislen;
285 int64_t hash = __grid_map_t_get_hash();
286
287 int64_t this_hash;
288 thislen =
289 __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
290 if (thislen < 0)
291 return thislen;
292 else
293 pos += thislen;
294 if (this_hash != hash)
295 return -1;
296
297 thislen = __grid_map_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
298 if (thislen < 0)
299 return thislen;
300 else
301 pos += thislen;
302
303 return pos;
304}
305
306int grid_map_t_decode_cleanup(grid_map_t *p) {
307 return __grid_map_t_decode_array_cleanup(p, 1);
308}
309
310int __grid_map_t_clone_array(const grid_map_t *p, grid_map_t *q, int elements) {
311 int element;
312 for (element = 0; element < elements; element++) {
313
314 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
315
316 __int8_t_clone_array(&(p[element].encoding), &(q[element].encoding), 1);
317
318 __double_clone_array(&(p[element].x0), &(q[element].x0), 1);
319
320 __double_clone_array(&(p[element].y0), &(q[element].y0), 1);
321
322 __double_clone_array(&(p[element].meters_per_pixel),
323 &(q[element].meters_per_pixel), 1);
324
325 __int32_t_clone_array(&(p[element].width), &(q[element].width), 1);
326
327 __int32_t_clone_array(&(p[element].height), &(q[element].height), 1);
328
329 __int32_t_clone_array(&(p[element].datalen), &(q[element].datalen), 1);
330
331 __string_clone_array(&(p[element].src), &(q[element].src), 1);
332
333 q[element].dst =
334 (uint8_t *)lcm_malloc(sizeof(uint8_t) * q[element].datalen);
335 __byte_clone_array(p[element].dst, q[element].dst, p[element].datalen);
336 }
337 return 0;
338}
339
340grid_map_t *grid_map_t_copy(const grid_map_t *p) {
341 grid_map_t *q = (grid_map_t *)malloc(sizeof(grid_map_t));
342 __grid_map_t_clone_array(p, q, 1);
343 return q;
344}
345
346void grid_map_t_destroy(grid_map_t *p) {
347 __grid_map_t_decode_array_cleanup(p, 1);
348 free(p);
349}
350
351int grid_map_t_publish(lcm_t *lc, const char *channel, const grid_map_t *p) {
352 int max_data_size = grid_map_t_encoded_size(p);
353 uint8_t *buf = (uint8_t *)malloc(max_data_size);
354 if (!buf)
355 return -1;
356 int data_size = grid_map_t_encode(buf, 0, max_data_size, p);
357 if (data_size < 0) {
358 free(buf);
359 return data_size;
360 }
361 int status = lcm_publish(lc, channel, buf, data_size);
362 free(buf);
363 return status;
364}
365
366struct _grid_map_t_subscription_t {
367 grid_map_t_handler_t user_handler;
368 void *userdata;
369 lcm_subscription_t *lc_h;
370};
371static void grid_map_t_handler_stub(const lcm_recv_buf_t *rbuf,
372 const char *channel, void *userdata) {
373 int status;
374 grid_map_t p;
375 memset(&p, 0, sizeof(grid_map_t));
376 status = grid_map_t_decode(rbuf->data, 0, rbuf->data_size, &p);
377 if (status < 0) {
378 fprintf(stderr, "error %d decoding grid_map_t!!!\n", status);
379 return;
380 }
381
382 grid_map_t_subscription_t *h = (grid_map_t_subscription_t *)userdata;
383 h->user_handler(rbuf, channel, &p, h->userdata);
384
385 grid_map_t_decode_cleanup(&p);
386}
387
388grid_map_t_subscription_t *grid_map_t_subscribe(lcm_t *lcm, const char *channel,
389 grid_map_t_handler_t f,
390 void *userdata) {
391 grid_map_t_subscription_t *n =
392 (grid_map_t_subscription_t *)malloc(sizeof(grid_map_t_subscription_t));
393 n->user_handler = f;
394 n->userdata = userdata;
395 n->lc_h = lcm_subscribe(lcm, channel, grid_map_t_handler_stub, n);
396 if (n->lc_h == NULL) {
397 fprintf(stderr, "couldn't reg grid_map_t LCM handler!\n");
398 free(n);
399 return NULL;
400 }
401 return n;
402}
403
404int grid_map_t_subscription_set_queue_capacity(grid_map_t_subscription_t *subs,
405 int num_messages) {
406 return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages);
407}
408
409int grid_map_t_unsubscribe(lcm_t *lcm, grid_map_t_subscription_t *hid) {
410 int status = lcm_unsubscribe(lcm, hid->lc_h);
411 if (0 != status) {
412 fprintf(stderr, "couldn't unsubscribe grid_map_t_handler %p!\n", hid);
413 return -1;
414 }
415 free(hid);
416 return 0;
417}
diff --git a/lcmtype/grid_map_t.h b/lcmtype/grid_map_t.h
new file mode 100644
index 0000000..7f91b91
--- /dev/null
+++ b/lcmtype/grid_map_t.h
@@ -0,0 +1,157 @@
1// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2// BY HAND!!
3//
4// Generated by lcm-gen
5
6// #include <lcm.h>
7// #include <lcm_coretypes.h>
8#include "lcmtype.h"
9#include <stdint.h>
10#include <stdlib.h>
11
12
13#ifndef _grid_map_t_h
14#define _grid_map_t_h
15
16#ifdef __cplusplus
17extern "C" {
18#endif
19
20typedef struct _grid_map_t grid_map_t;
21struct _grid_map_t {
22 int64_t utime;
23 int8_t encoding;
24 double x0;
25 double y0;
26 double meters_per_pixel;
27 int32_t width;
28 int32_t height;
29 int32_t datalen;
30 char *src;
31 uint8_t *dst;
32};
33
34/**
35 * Create a deep copy of a grid_map_t.
36 * When no longer needed, destroy it with grid_map_t_destroy()
37 */
38grid_map_t *grid_map_t_copy(const grid_map_t *to_copy);
39
40/**
41 * Destroy an instance of grid_map_t created by grid_map_t_copy()
42 */
43void grid_map_t_destroy(grid_map_t *to_destroy);
44
45/**
46 * Identifies a single subscription. This is an opaque data type.
47 */
48typedef struct _grid_map_t_subscription_t grid_map_t_subscription_t;
49
50/**
51 * Prototype for a callback function invoked when a message of type
52 * grid_map_t is received.
53 */
54typedef void (*grid_map_t_handler_t)(const lcm_recv_buf_t *rbuf,
55 const char *channel, const grid_map_t *msg,
56 void *userdata);
57
58/**
59 * Publish a message of type grid_map_t using LCM.
60 *
61 * @param lcm The LCM instance to publish with.
62 * @param channel The channel to publish on.
63 * @param msg The message to publish.
64 * @return 0 on success, <0 on error. Success means LCM has transferred
65 * responsibility of the message data to the OS.
66 */
67int grid_map_t_publish(lcm_t *lcm, const char *channel, const grid_map_t *msg);
68
69/**
70 * Subscribe to messages of type grid_map_t using LCM.
71 *
72 * @param lcm The LCM instance to subscribe with.
73 * @param channel The channel to subscribe to.
74 * @param handler The callback function invoked by LCM when a message is
75 * received. This function is invoked by LCM during calls to lcm_handle() and
76 * lcm_handle_timeout().
77 * @param userdata An opaque pointer passed to @p handler when it is invoked.
78 * @return 0 on success, <0 if an error occured
79 */
80grid_map_t_subscription_t *grid_map_t_subscribe(lcm_t *lcm, const char *channel,
81 grid_map_t_handler_t handler,
82 void *userdata);
83
84/**
85 * Removes and destroys a subscription created by grid_map_t_subscribe()
86 */
87int grid_map_t_unsubscribe(lcm_t *lcm, grid_map_t_subscription_t *hid);
88
89/**
90 * Sets the queue capacity for a subscription.
91 * Some LCM providers (e.g., the default multicast provider) are implemented
92 * using a background receive thread that constantly revceives messages from
93 * the network. As these messages are received, they are buffered on
94 * per-subscription queues until dispatched by lcm_handle(). This function
95 * how many messages are queued before dropping messages.
96 *
97 * @param subs the subscription to modify.
98 * @param num_messages The maximum number of messages to queue
99 * on the subscription.
100 * @return 0 on success, <0 if an error occured
101 */
102int grid_map_t_subscription_set_queue_capacity(grid_map_t_subscription_t *subs,
103 int num_messages);
104
105/**
106 * Encode a message of type grid_map_t into binary form.
107 *
108 * @param buf The output buffer.
109 * @param offset Encoding starts at this byte offset into @p buf.
110 * @param maxlen Maximum number of bytes to write. This should generally
111 * be equal to grid_map_t_encoded_size().
112 * @param msg The message to encode.
113 * @return The number of bytes encoded, or <0 if an error occured.
114 */
115int grid_map_t_encode(void *buf, int offset, int maxlen, const grid_map_t *p);
116
117/**
118 * Decode a message of type grid_map_t from binary form.
119 * When decoding messages containing strings or variable-length arrays, this
120 * function may allocate memory. When finished with the decoded message,
121 * release allocated resources with grid_map_t_decode_cleanup().
122 *
123 * @param buf The buffer containing the encoded message
124 * @param offset The byte offset into @p buf where the encoded message starts.
125 * @param maxlen The maximum number of bytes to read while decoding.
126 * @param msg Output parameter where the decoded message is stored
127 * @return The number of bytes decoded, or <0 if an error occured.
128 */
129int grid_map_t_decode(const void *buf, int offset, int maxlen, grid_map_t *msg);
130
131/**
132 * Release resources allocated by grid_map_t_decode()
133 * @return 0
134 */
135int grid_map_t_decode_cleanup(grid_map_t *p);
136
137/**
138 * Check how many bytes are required to encode a message of type grid_map_t
139 */
140int grid_map_t_encoded_size(const grid_map_t *p);
141
142// LCM support functions. Users should not call these
143int64_t __grid_map_t_get_hash(void);
144uint64_t __grid_map_t_hash_recursive(const __lcm_hash_ptr *p);
145int __grid_map_t_encode_array(void *buf, int offset, int maxlen,
146 const grid_map_t *p, int elements);
147int __grid_map_t_decode_array(const void *buf, int offset, int maxlen,
148 grid_map_t *p, int elements);
149int __grid_map_t_decode_array_cleanup(grid_map_t *p, int elements);
150int __grid_map_t_encoded_array_size(const grid_map_t *p, int elements);
151int __grid_map_t_clone_array(const grid_map_t *p, grid_map_t *q, int elements);
152
153#ifdef __cplusplus
154}
155#endif
156
157#endif
diff --git a/lcmtype/laser_t.cpp b/lcmtype/laser_t.cpp
new file mode 100644
index 0000000..23dca66
--- /dev/null
+++ b/lcmtype/laser_t.cpp
@@ -0,0 +1,364 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "laser_t.h"
8#include <string.h>
9
10static int __laser_t_hash_computed;
11static int64_t __laser_t_hash;
12
13int64_t __laser_t_hash_recursive(const __lcm_hash_ptr *p) {
14 const __lcm_hash_ptr *fp;
15 for (fp = p; fp != NULL; fp = fp->parent)
16 if (fp->v == __laser_t_get_hash)
17 return 0;
18
19 const __lcm_hash_ptr cp = {p, (void *)__laser_t_get_hash};
20 (void)cp;
21
22 int64_t hash = 0xf1e8ba118c05af46LL + __int64_t_hash_recursive(&cp) +
23 __int32_t_hash_recursive(&cp) + __float_hash_recursive(&cp) +
24 __int32_t_hash_recursive(&cp) + __float_hash_recursive(&cp) +
25 __float_hash_recursive(&cp) + __float_hash_recursive(&cp);
26
27 return (hash << 1) + ((hash >> 63) & 1);
28}
29
30int64_t __laser_t_get_hash(void) {
31 if (!__laser_t_hash_computed) {
32 __laser_t_hash = __laser_t_hash_recursive(NULL);
33 __laser_t_hash_computed = 1;
34 }
35
36 return __laser_t_hash;
37}
38
39int __laser_t_encode_array(void *buf, int offset, int maxlen, const laser_t *p,
40 int elements) {
41 int pos = 0, thislen, element;
42
43 for (element = 0; element < elements; element++) {
44
45 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
46 &(p[element].utime), 1);
47 if (thislen < 0)
48 return thislen;
49 else
50 pos += thislen;
51
52 thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos,
53 &(p[element].nranges), 1);
54 if (thislen < 0)
55 return thislen;
56 else
57 pos += thislen;
58
59 thislen = __float_encode_array(buf, offset + pos, maxlen - pos,
60 p[element].ranges, p[element].nranges);
61 if (thislen < 0)
62 return thislen;
63 else
64 pos += thislen;
65
66 thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos,
67 &(p[element].nintensities), 1);
68 if (thislen < 0)
69 return thislen;
70 else
71 pos += thislen;
72
73 thislen = __float_encode_array(buf, offset + pos, maxlen - pos,
74 p[element].intensities,
75 p[element].nintensities);
76 if (thislen < 0)
77 return thislen;
78 else
79 pos += thislen;
80
81 thislen = __float_encode_array(buf, offset + pos, maxlen - pos,
82 &(p[element].rad0), 1);
83 if (thislen < 0)
84 return thislen;
85 else
86 pos += thislen;
87
88 thislen = __float_encode_array(buf, offset + pos, maxlen - pos,
89 &(p[element].radstep), 1);
90 if (thislen < 0)
91 return thislen;
92 else
93 pos += thislen;
94 }
95 return pos;
96}
97
98int laser_t_encode(void *buf, int offset, int maxlen, const laser_t *p) {
99 int pos = 0, thislen;
100 int64_t hash = __laser_t_get_hash();
101
102 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
103 if (thislen < 0)
104 return thislen;
105 else
106 pos += thislen;
107
108 thislen = __laser_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
109 if (thislen < 0)
110 return thislen;
111 else
112 pos += thislen;
113
114 return pos;
115}
116
117int __laser_t_encoded_array_size(const laser_t *p, int elements) {
118 int size = 0, element;
119 for (element = 0; element < elements; element++) {
120
121 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
122
123 size += __int32_t_encoded_array_size(&(p[element].nranges), 1);
124
125 size +=
126 __float_encoded_array_size(p[element].ranges, p[element].nranges);
127
128 size += __int32_t_encoded_array_size(&(p[element].nintensities), 1);
129
130 size += __float_encoded_array_size(p[element].intensities,
131 p[element].nintensities);
132
133 size += __float_encoded_array_size(&(p[element].rad0), 1);
134
135 size += __float_encoded_array_size(&(p[element].radstep), 1);
136 }
137 return size;
138}
139
140int laser_t_encoded_size(const laser_t *p) {
141 return 8 + __laser_t_encoded_array_size(p, 1);
142}
143
144int __laser_t_decode_array(const void *buf, int offset, int maxlen, laser_t *p,
145 int elements) {
146 int pos = 0, thislen, element;
147
148 for (element = 0; element < elements; element++) {
149
150 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
151 &(p[element].utime), 1);
152 if (thislen < 0)
153 return thislen;
154 else
155 pos += thislen;
156
157 thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos,
158 &(p[element].nranges), 1);
159 if (thislen < 0)
160 return thislen;
161 else
162 pos += thislen;
163
164 p[element].ranges =
165 (float *)lcm_malloc(sizeof(float) * p[element].nranges);
166 thislen = __float_decode_array(buf, offset + pos, maxlen - pos,
167 p[element].ranges, p[element].nranges);
168 if (thislen < 0)
169 return thislen;
170 else
171 pos += thislen;
172
173 thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos,
174 &(p[element].nintensities), 1);
175 if (thislen < 0)
176 return thislen;
177 else
178 pos += thislen;
179
180 p[element].intensities =
181 (float *)lcm_malloc(sizeof(float) * p[element].nintensities);
182 thislen = __float_decode_array(buf, offset + pos, maxlen - pos,
183 p[element].intensities,
184 p[element].nintensities);
185 if (thislen < 0)
186 return thislen;
187 else
188 pos += thislen;
189
190 thislen = __float_decode_array(buf, offset + pos, maxlen - pos,
191 &(p[element].rad0), 1);
192 if (thislen < 0)
193 return thislen;
194 else
195 pos += thislen;
196
197 thislen = __float_decode_array(buf, offset + pos, maxlen - pos,
198 &(p[element].radstep), 1);
199 if (thislen < 0)
200 return thislen;
201 else
202 pos += thislen;
203 }
204 return pos;
205}
206
207int __laser_t_decode_array_cleanup(laser_t *p, int elements) {
208 int element;
209 for (element = 0; element < elements; element++) {
210
211 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
212
213 __int32_t_decode_array_cleanup(&(p[element].nranges), 1);
214
215 __float_decode_array_cleanup(p[element].ranges, p[element].nranges);
216 if (p[element].ranges)
217 free(p[element].ranges);
218
219 __int32_t_decode_array_cleanup(&(p[element].nintensities), 1);
220
221 __float_decode_array_cleanup(p[element].intensities,
222 p[element].nintensities);
223 if (p[element].intensities)
224 free(p[element].intensities);
225
226 __float_decode_array_cleanup(&(p[element].rad0), 1);
227
228 __float_decode_array_cleanup(&(p[element].radstep), 1);
229 }
230 return 0;
231}
232
233int laser_t_decode(const void *buf, int offset, int maxlen, laser_t *p) {
234 int pos = 0, thislen;
235 int64_t hash = __laser_t_get_hash();
236
237 int64_t this_hash;
238 thislen =
239 __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
240 if (thislen < 0)
241 return thislen;
242 else
243 pos += thislen;
244 if (this_hash != hash)
245 return -1;
246
247 thislen = __laser_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
248 if (thislen < 0)
249 return thislen;
250 else
251 pos += thislen;
252
253 return pos;
254}
255
256int laser_t_decode_cleanup(laser_t *p) {
257 return __laser_t_decode_array_cleanup(p, 1);
258}
259
260int __laser_t_clone_array(const laser_t *p, laser_t *q, int elements) {
261 int element;
262 for (element = 0; element < elements; element++) {
263
264 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
265
266 __int32_t_clone_array(&(p[element].nranges), &(q[element].nranges), 1);
267
268 q[element].ranges =
269 (float *)lcm_malloc(sizeof(float) * q[element].nranges);
270 __float_clone_array(p[element].ranges, q[element].ranges,
271 p[element].nranges);
272
273 __int32_t_clone_array(&(p[element].nintensities),
274 &(q[element].nintensities), 1);
275
276 q[element].intensities =
277 (float *)lcm_malloc(sizeof(float) * q[element].nintensities);
278 __float_clone_array(p[element].intensities, q[element].intensities,
279 p[element].nintensities);
280
281 __float_clone_array(&(p[element].rad0), &(q[element].rad0), 1);
282
283 __float_clone_array(&(p[element].radstep), &(q[element].radstep), 1);
284 }
285 return 0;
286}
287
288laser_t *laser_t_copy(const laser_t *p) {
289 laser_t *q = (laser_t *)malloc(sizeof(laser_t));
290 __laser_t_clone_array(p, q, 1);
291 return q;
292}
293
294void laser_t_destroy(laser_t *p) {
295 __laser_t_decode_array_cleanup(p, 1);
296 free(p);
297}
298
299int laser_t_publish(lcm_t *lc, const char *channel, const laser_t *p) {
300 int max_data_size = laser_t_encoded_size(p);
301 uint8_t *buf = (uint8_t *)malloc(max_data_size);
302 if (!buf)
303 return -1;
304 int data_size = laser_t_encode(buf, 0, max_data_size, p);
305 if (data_size < 0) {
306 free(buf);
307 return data_size;
308 }
309 int status = lcm_publish(lc, channel, buf, data_size);
310 free(buf);
311 return status;
312}
313
314struct _laser_t_subscription_t {
315 laser_t_handler_t user_handler;
316 void *userdata;
317 lcm_subscription_t *lc_h;
318};
319static void laser_t_handler_stub(const lcm_recv_buf_t *rbuf,
320 const char *channel, void *userdata) {
321 int status;
322 laser_t p;
323 memset(&p, 0, sizeof(laser_t));
324 status = laser_t_decode(rbuf->data, 0, rbuf->data_size, &p);
325 if (status < 0) {
326 fprintf(stderr, "error %d decoding laser_t!!!\n", status);
327 return;
328 }
329
330 laser_t_subscription_t *h = (laser_t_subscription_t *)userdata;
331 h->user_handler(rbuf, channel, &p, h->userdata);
332
333 laser_t_decode_cleanup(&p);
334}
335
336laser_t_subscription_t *laser_t_subscribe(lcm_t *lcm, const char *channel,
337 laser_t_handler_t f, void *userdata) {
338 laser_t_subscription_t *n =
339 (laser_t_subscription_t *)malloc(sizeof(laser_t_subscription_t));
340 n->user_handler = f;
341 n->userdata = userdata;
342 n->lc_h = lcm_subscribe(lcm, channel, laser_t_handler_stub, n);
343 if (n->lc_h == NULL) {
344 fprintf(stderr, "couldn't reg laser_t LCM handler!\n");
345 free(n);
346 return NULL;
347 }
348 return n;
349}
350
351int laser_t_subscription_set_queue_capacity(laser_t_subscription_t *subs,
352 int num_messages) {
353 return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages);
354}
355
356int laser_t_unsubscribe(lcm_t *lcm, laser_t_subscription_t *hid) {
357 int status = lcm_unsubscribe(lcm, hid->lc_h);
358 if (0 != status) {
359 fprintf(stderr, "couldn't unsubscribe laser_t_handler %p!\n", hid);
360 return -1;
361 }
362 free(hid);
363 return 0;
364}
diff --git a/lcmtype/laser_t.h b/lcmtype/laser_t.h
new file mode 100644
index 0000000..f3744ae
--- /dev/null
+++ b/lcmtype/laser_t.h
@@ -0,0 +1,67 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "lcm.h"
8#include "lcmtype.h"
9#include <stdint.h>
10#include <stdlib.h>
11
12#ifndef _laser_t_h
13#define _laser_t_h
14
15// 若为C++代码，需加上extern "C"，表明以下按照C的方式进行编译
16// 否则会出现链接错误
17#ifdef __cplusplus
18extern "C" {
19#endif
20
21typedef struct _laser_t laser_t;
22struct _laser_t {
23 int64_t utime;
24 int32_t nranges;
25 float *ranges;
26 int32_t nintensities;
27 float *intensities;
28 float rad0;
29 float radstep;
30};
31
32laser_t *laser_t_copy(const laser_t *p);
33void laser_t_destroy(laser_t *p);
34
35typedef struct _laser_t_subscription_t laser_t_subscription_t;
36typedef void (*laser_t_handler_t)(const lcm_recv_buf_t *rbuf,
37 const char *channel, const laser_t *msg,
38 void *user);
39
40int laser_t_publish(lcm_t *lcm, const char *channel, const laser_t *p);
41laser_t_subscription_t *laser_t_subscribe(lcm_t *lcm, const char *channel,
42 laser_t_handler_t f, void *userdata);
43int laser_t_unsubscribe(lcm_t *lcm, laser_t_subscription_t *hid);
44int laser_t_subscription_set_queue_capacity(laser_t_subscription_t *subs,
45 int num_messages);
46
47int laser_t_encode(void *buf, int offset, int maxlen, const laser_t *p);
48int laser_t_decode(const void *buf, int offset, int maxlen, laser_t *p);
49int laser_t_decode_cleanup(laser_t *p);
50int laser_t_encoded_size(const laser_t *p);
51
52// LCM support functions. Users should not call these
53int64_t __laser_t_get_hash(void);
54int64_t __laser_t_hash_recursive(const __lcm_hash_ptr *p);
55int __laser_t_encode_array(void *buf, int offset, int maxlen, const laser_t *p,
56 int elements);
57int __laser_t_decode_array(const void *buf, int offset, int maxlen, laser_t *p,
58 int elements);
59int __laser_t_decode_array_cleanup(laser_t *p, int elements);
60int __laser_t_encoded_array_size(const laser_t *p, int elements);
61int __laser_t_clone_array(const laser_t *p, laser_t *q, int elements);
62
63#ifdef __cplusplus
64}
65#endif
66
67#endif
diff --git a/lcmtype/lcm.h b/lcmtype/lcm.h
new file mode 100644
index 0000000..d3762e9
--- /dev/null
+++ b/lcmtype/lcm.h
@@ -0,0 +1,316 @@
1#ifndef __lightweight_comunications_h__
2#define __lightweight_comunications_h__
3
4#ifdef __cplusplus
5extern "C" {
6#endif
7
8#include <stdint.h>
9
10#include "eventlog.h"
11
12#define LCM_MAX_MESSAGE_SIZE (1 << 28)
13
14#define LCM_MAX_CHANNEL_NAME_LENGTH 63
15
16#ifdef WIN32
17#define LCM_API_FUNCTION __declspec(dllexport)
18#else
19#define LCM_API_FUNCTION
20#endif
21
22/**
23 * @defgroup LcmC C API Reference
24 *
25 * THe %LCM C API provides classes and data structures for communicating with
26 * other %LCM clients, as well as reading and writing %LCM log files.
27 *
28 */
29
30/**
31 * @defgroup LcmC_lcm_t lcm_t
32 * @ingroup LcmC
33 * @brief Publish and receive messages
34 *
35 * All %LCM functions are internally synchronized and thread-safe.
36 *
37 * <tt> #include <lcm.h> </tt>
38 *
39 * Linking: <tt> `pkg-config --libs lcm` </tt>
40 * @{
41 */
42
43/**
44 * Opaque data structure containing the LCM context.
45 */
46typedef struct _lcm_t lcm_t;
47
48/**
49 * An opaque data structure that identifies an LCM subscription.
50 */
51typedef struct _lcm_subscription_t lcm_subscription_t;
52
53/**
54 * Received messages are passed to user programs using this data structure.
55 * Each instance represents one message.
56 */
57typedef struct _lcm_recv_buf_t lcm_recv_buf_t;
58struct _lcm_recv_buf_t {
59 /**
60 * the data received (raw bytes)
61 */
62 void *data;
63 /**
64 * the length of the data received (in bytes)
65 */
66 uint32_t data_size;
67 /**
68 * timestamp (micrseconds since the epoch) at which the first data
69 * bytes of the message were received.
70 */
71 int64_t recv_utime;
72 /**
73 * pointer to the lcm_t struct that owns this buffer
74 */
75 lcm_t *lcm;
76};
77
78/**
79 * @brief Callback function prototype.
80 *
81 * Pass instances of this to lcm_subscribe()
82 *
83 * @param rbuf the message timestamp and payload
84 * @param channel the channel the message was received on
85 * @param user_data the user-specified parameter passed to lcm_subscribe()
86 */
87typedef void (*lcm_msg_handler_t)(const lcm_recv_buf_t *rbuf,
88 const char *channel, void *user_data);
89
90/**
91 * @brief Constructor
92 *
93 * Allocates and initializes a lcm_t. %provider must be either
94 * NULL, or a string of the form
95 *
96 * <tt>"provider://network?option1=value1&option2=value2&...&optionN=valueN"</tt>
97 *
98 * @param provider Initializationg string specifying the LCM network provider.
99 * If this is NULL, and the environment variable "LCM_DEFAULT_URL" is defined,
100 * then the environment variable is used instead. If this is NULL and the
101 * environment variable is not defined, then default settings are used.
102 *
103 * The currently supported providers are:
104 *
105 * @verbatim
106 udpm://
107 UDP Multicast provider
108 network can be of the form "multicast_address:port". Either the
109 multicast address or the port may be ommitted for the default.
110
111 options:
112 recv_buf_size = N
113 size of the kernel UDP receive buffer to request. Defaults to
114 operating system defaults
115
116 ttl = N
117 time to live of transmitted packets. Default 0
118
119 examples:
120 "udpm://239.255.76.67:7667"
121 Default initialization string
122
123 "udpm://239.255.76.67:7667?ttl=1"
124 Sets the multicast TTL to 1 so that packets published will enter
125 the local network.
126 @endverbatim
127 *
128 * @verbatim
129 file://
130 LCM Log file-based provider
131 network should be the path to the log file
132
133 Events are read from or written to the log file. In read mode, events
134 are generated from the log file in real-time, or at the rate specified
135 by the speed option. In write mode, events published to the LCM instance
136 will be written to the log file in real-time.
137
138 options:
139 speed = N
140 Scale factor controlling the playback speed of the log file.
141 Defaults to 1. If less than or equal to zero, then the events
142 in the log file are played back as fast as possible. Events are
143 never skipped in read mode, so actual playback speed may be slower
144 than requested, depending on the handlers.
145
146 mode = r | w
147 Specifies the log file mode. Defaults to 'r'
148
149 start_timestamp = USEC
150 Seeks to USEC microseconds in the logfile, where USEC is given in
151 microseconds since 00:00:00 UTC on 1 January 1970. If USEC is
152 before the first event, then playback begins at the start of the
153 log file. If it is after the last event, calls to lcm_handle will
154 return -1.
155
156 examples:
157 "file:///home/albert/path/to/logfile"
158 Loads the file "/home/albert/path/to/logfile" as an LCM event
159 source.
160
161 "file:///home/albert/path/to/logfile?speed=4"
162 Loads the file "/home/albert/path/to/logfile" as an LCM event
163 source. Events are played back at 4x speed.
164
165 @endverbatim
166 *
167 * @return a newly allocated lcm_t instance, or NULL on failure. Free with
168 * lcm_destroy() when no longer needed.
169 */
170LCM_API_FUNCTION
171lcm_t *lcm_create(const char *provider);
172
173/**
174 * @brief Destructor
175 */
176LCM_API_FUNCTION
177void lcm_destroy(lcm_t *lcm);
178
179/**
180 * @brief Returns a file descriptor or socket that can be used with
181 * @c select(), @c poll(), or other event loops for asynchronous
182 * notification of incoming messages.
183 *
184 * Each LCM instance has a file descriptor that can be used to asynchronously
185 * receive notification when incoming messages have been received. This file
186 * descriptor can typically be incorporated into an application event loop
187 * (e.g., GTK+, QT, etc.) For an example using select(), see
188 * examples/c/listener-async.c
189 *
190 * @return a file descriptor suitable for use with select, poll, etc.
191 */
192LCM_API_FUNCTION
193int lcm_get_fileno(lcm_t *lcm);
194
195/**
196 * @brief Subscribe a callback function to a channel, without automatic message
197 * decoding.
198 *
199 * In general, you probably don't want to use this function, as it does not
200 * automatically decode messages. Instead, use the message-specific subscribe
201 * function generated by @c lcm-gen. Use this function only when you want to
202 * work with the raw message itself. TODO link to example or more details.
203 *
204 * The callback function will be invoked during calls to lcm_handle() any time
205 * a message on the specified channel is received. Multiple callbacks can be
206 * subscribed for the same channel.
207 *
208 * @param lcm the LCM object
209 * @param channel the channel to listen on. This can also be a GLib regular
210 * expression, and is treated as a regex implicitly surrounded
211 * by '^' and '$'
212 * @param handler the callback function to be invoked when a message is
213 * received on the specified channel
214 * @param userdata this will be passed to the callback function
215 *
216 * @return a lcm_subscription_t to identify the new subscription,
217 * which can be passed to lcm_unsubscribe(). The lcm_t instance owns
218 * the subscription object.
219 */
220LCM_API_FUNCTION
221lcm_subscription_t *lcm_subscribe(lcm_t *lcm, const char *channel,
222 lcm_msg_handler_t handler, void *userdata);
223
224/**
225 * @brief Unsubscribe a message handler.
226 *
227 * In general, you probably don't want to use this function. Instead, use the
228 * message-specific unsubscribe function generated by @c lcm-gen. Use this
229 * function only when you want to work with the raw message itself. TODO link
230 * to example or more details.
231 *
232 * The callback function for the subscription will no longer be
233 * invoked when messages on the corresponding channel are received. After this
234 * function returns, @c handler is no longer valid and should not be used
235 * anymore.
236 *
237 * @return 0 on success, or -1 if @c handler is not a valid subscription.
238 */
239LCM_API_FUNCTION
240int lcm_unsubscribe(lcm_t *lcm, lcm_subscription_t *handler);
241
242/**
243 * @brief Publish a message, specified as a raw byte buffer.
244 *
245 * In general, you probably don't want to use this function, as it does not
246 * automatically encode messages. Instead, use the message-specific publish
247 * function generated by @c lcm-gen. Use this function only when you want to
248 * publish raw byte buffers. TODO link to example or more details.
249 *
250 * @param lcm The %LCM object
251 * @param channel The channel to publish on
252 * @param data The raw byte buffer
253 * @param datalen Size of the byte buffer
254 *
255 * @return 0 on success, -1 on failure.
256 */
257LCM_API_FUNCTION
258int lcm_publish(lcm_t *lcm, const char *channel, const void *data,
259 unsigned int datalen);
260
261/**
262 * @brief Wait for and dispatch the next incoming message.
263 *
264 * Message handlers are invoked one at a time from the thread that calls this
265 * function, and in the order that they were subscribed.
266 *
267 * This function waits indefinitely. If you want timeout behavior, (e.g., wait
268 * 100ms for a message) then consider using lcm_get_fileno() together with
269 * select() or poll()
270 *
271 * Recursive calls to lcm_handle are not allowed -- do not call lcm_handle from
272 * within a message handler. All other functions are okay (e.g., it is okay to
273 * call lcm_publish from within a message handler).
274 *
275 * @param lcm the %LCM object
276 *
277 * @return 0 normally, or -1 when an error has occurred.
278 */
279LCM_API_FUNCTION
280int lcm_handle(lcm_t *lcm);
281
282/**
283 * @brief Adjusts the maximum number of received messages that can be queued up
284 * for a subscription.
285 *
286 * In general, you probably don't want to use this function. Instead, use the
287 * message-specific set_queue_capacity function generated by @c lcm-gen. Use
288 * this function only when you want to work with untyped subscriptions. TODO
289 * link to example or more details.
290 *
291 * Setting this to a low number may reduce overall latency at the expense of
292 * dropping more messages. Conversely, setting this to a high number may drop
293 * fewer messages at the expense of increased latency. A value of 0 indicates
294 * no limit, and should be used carefully.
295 *
296 * @param handler the subscription object
297 * @param num_messages the maximum queue size, in messages. The default is 30.
298 *
299 */
300LCM_API_FUNCTION
301int lcm_subscription_set_queue_capacity(lcm_subscription_t *handler,
302 int num_messages);
303
304#define LCM_MAJOR_VERSION 1
305#define LCM_MINOR_VERSION 0
306#define LCM_MICRO_VERSION 0
307
308/**
309 * @}
310 */
311
312#ifdef __cplusplus
313}
314#endif
315
316#endif
diff --git a/lcmtype/lcm_coretypes.h b/lcmtype/lcm_coretypes.h
new file mode 100644
index 0000000..c4208e1
--- /dev/null
+++ b/lcmtype/lcm_coretypes.h
@@ -0,0 +1,460 @@
1#ifndef _LCM_LIB_INLINE_H
2#define _LCM_LIB_INLINE_H
3
4#include <stdint.h>
5#include <stdio.h>
6#include <stdlib.h>
7#include <string.h>
8
9#ifdef __cplusplus
10extern "C" {
11#endif
12
13union float_uint32 {
14 float f;
15 uint32_t i;
16};
17
18union double_uint64 {
19 double f;
20 uint64_t i;
21};
22
23typedef struct ___lcm_hash_ptr __lcm_hash_ptr;
24struct ___lcm_hash_ptr {
25 const __lcm_hash_ptr *parent;
26 void *v;
27};
28
29/**
30 * BOOLEAN
31 */
32#define __boolean_hash_recursive __int8_t_hash_recursive
33#define __boolean_decode_array_cleanup __int8_t_decode_array_cleanup
34#define __boolean_encoded_array_size __int8_t_encoded_array_size
35#define __boolean_encode_array __int8_t_encode_array
36#define __boolean_decode_array __int8_t_decode_array
37#define __boolean_clone_array __int8_t_clone_array
38#define boolean_encoded_size int8_t_encoded_size
39
40/**
41 * BYTE
42 */
43#define __byte_hash_recursive(p) 0
44#define __byte_decode_array_cleanup(p, sz) \
45 {}
46#define byte_encoded_size(p) (sizeof(int64_t) + sizeof(uint8_t))
47
48static inline int __byte_encoded_array_size(const uint8_t *p, int elements) {
49 (void)p;
50 return sizeof(uint8_t) * elements;
51}
52
53static inline int __byte_encode_array(void *_buf, int offset, int maxlen,
54 const uint8_t *p, int elements) {
55 if (maxlen < elements)
56 return -1;
57
58 uint8_t *buf = (uint8_t *)_buf;
59 memcpy(&buf[offset], p, elements);
60
61 return elements;
62}
63
64static inline int __byte_decode_array(const void *_buf, int offset, int maxlen,
65 uint8_t *p, int elements) {
66 if (maxlen < elements)
67 return -1;
68
69 uint8_t *buf = (uint8_t *)_buf;
70 memcpy(p, &buf[offset], elements);
71
72 return elements;
73}
74
75static inline int __byte_clone_array(const uint8_t *p, uint8_t *q,
76 int elements) {
77 memcpy(q, p, elements * sizeof(uint8_t));
78 return 0;
79}
80/**
81 * INT8_T
82 */
83#define __int8_t_hash_recursive(p) 0
84#define __int8_t_decode_array_cleanup(p, sz) \
85 {}
86#define int8_t_encoded_size(p) (sizeof(int64_t) + sizeof(int8_t))
87
88static inline int __int8_t_encoded_array_size(const int8_t *p, int elements) {
89 (void)p;
90 return sizeof(int8_t) * elements;
91}
92
93static inline int __int8_t_encode_array(void *_buf, int offset, int maxlen,
94 const int8_t *p, int elements) {
95 if (maxlen < elements)
96 return -1;
97
98 int8_t *buf = (int8_t *)_buf;
99 memcpy(&buf[offset], p, elements);
100
101 return elements;
102}
103
104static inline int __int8_t_decode_array(const void *_buf, int offset,
105 int maxlen, int8_t *p, int elements) {
106 if (maxlen < elements)
107 return -1;
108
109 int8_t *buf = (int8_t *)_buf;
110 memcpy(p, &buf[offset], elements);
111
112 return elements;
113}
114
115static inline int __int8_t_clone_array(const int8_t *p, int8_t *q,
116 int elements) {
117 memcpy(q, p, elements * sizeof(int8_t));
118 return 0;
119}
120
121/**
122 * INT16_T
123 */
124#define __int16_t_hash_recursive(p) 0
125#define __int16_t_decode_array_cleanup(p, sz) \
126 {}
127#define int16_t_encoded_size(p) (sizeof(int64_t) + sizeof(int16_t))
128
129static inline int __int16_t_encoded_array_size(const int16_t *p, int elements) {
130 (void)p;
131 return sizeof(int16_t) * elements;
132}
133
134static inline int __int16_t_encode_array(void *_buf, int offset, int maxlen,
135 const int16_t *p, int elements) {
136 int total_size = sizeof(int16_t) * elements;
137 uint8_t *buf = (uint8_t *)_buf;
138 int pos = offset;
139 int element;
140
141 if (maxlen < total_size)
142 return -1;
143
144 for (element = 0; element < elements; element++) {
145 int16_t v = p[element];
146 buf[pos++] = (v >> 8) & 0xff;
147 buf[pos++] = (v & 0xff);
148 }
149
150 return total_size;
151}
152
153static inline int __int16_t_decode_array(const void *_buf, int offset,
154 int maxlen, int16_t *p, int elements) {
155 int total_size = sizeof(int16_t) * elements;
156 uint8_t *buf = (uint8_t *)_buf;
157 int pos = offset;
158 int element;
159
160 if (maxlen < total_size)
161 return -1;
162
163 for (element = 0; element < elements; element++) {
164 p[element] = (buf[pos] << 8) + buf[pos + 1];
165 pos += 2;
166 }
167
168 return total_size;
169}
170
171static inline int __int16_t_clone_array(const int16_t *p, int16_t *q,
172 int elements) {
173 memcpy(q, p, elements * sizeof(int16_t));
174 return 0;
175}
176
177/**
178 * INT32_T
179 */
180#define __int32_t_hash_recursive(p) 0
181#define __int32_t_decode_array_cleanup(p, sz) \
182 {}
183#define int32_t_encoded_size(p) (sizeof(int64_t) + sizeof(int32_t))
184
185static inline int __int32_t_encoded_array_size(const int32_t *p, int elements) {
186 (void)p;
187 return sizeof(int32_t) * elements;
188}
189
190static inline int __int32_t_encode_array(void *_buf, int offset, int maxlen,
191 const int32_t *p, int elements) {
192 int total_size = sizeof(int32_t) * elements;
193 uint8_t *buf = (uint8_t *)_buf;
194 int pos = offset;
195 int element;
196
197 if (maxlen < total_size)
198 return -1;
199
200 for (element = 0; element < elements; element++) {
201 int32_t v = p[element];
202 buf[pos++] = (v >> 24) & 0xff;
203 buf[pos++] = (v >> 16) & 0xff;
204 buf[pos++] = (v >> 8) & 0xff;
205 buf[pos++] = (v & 0xff);
206 }
207
208 return total_size;
209}
210
211static inline int __int32_t_decode_array(const void *_buf, int offset,
212 int maxlen, int32_t *p, int elements) {
213 int total_size = sizeof(int32_t) * elements;
214 uint8_t *buf = (uint8_t *)_buf;
215 int pos = offset;
216 int element;
217
218 if (maxlen < total_size)
219 return -1;
220
221 for (element = 0; element < elements; element++) {
222 p[element] = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) +
223 (buf[pos + 2] << 8) + buf[pos + 3];
224 pos += 4;
225 }
226
227 return total_size;
228}
229
230static inline int __int32_t_clone_array(const int32_t *p, int32_t *q,
231 int elements) {
232 memcpy(q, p, elements * sizeof(int32_t));
233 return 0;
234}
235
236/**
237 * INT64_T
238 */
239#define __int64_t_hash_recursive(p) 0
240#define __int64_t_decode_array_cleanup(p, sz) \
241 {}
242#define int64_t_encoded_size(p) (sizeof(int64_t) + sizeof(int64_t))
243
244static inline int __int64_t_encoded_array_size(const int64_t *p, int elements) {
245 (void)p;
246 return sizeof(int64_t) * elements;
247}
248
249static inline int __int64_t_encode_array(void *_buf, int offset, int maxlen,
250 const int64_t *p, int elements) {
251 int total_size = sizeof(int64_t) * elements;
252 uint8_t *buf = (uint8_t *)_buf;
253 int pos = offset;
254 int element;
255
256 if (maxlen < total_size)
257 return -1;
258
259 for (element = 0; element < elements; element++) {
260 int64_t v = p[element];
261 buf[pos++] = (v >> 56) & 0xff;
262 buf[pos++] = (v >> 48) & 0xff;
263 buf[pos++] = (v >> 40) & 0xff;
264 buf[pos++] = (v >> 32) & 0xff;
265 buf[pos++] = (v >> 24) & 0xff;
266 buf[pos++] = (v >> 16) & 0xff;
267 buf[pos++] = (v >> 8) & 0xff;
268 buf[pos++] = (v & 0xff);
269 }
270
271 return total_size;
272}
273
274static inline int __int64_t_decode_array(const void *_buf, int offset,
275 int maxlen, int64_t *p, int elements) {
276 int total_size = sizeof(int64_t) * elements;
277 uint8_t *buf = (uint8_t *)_buf;
278 int pos = offset;
279 int element;
280
281 if (maxlen < total_size)
282 return -1;
283
284 for (element = 0; element < elements; element++) {
285 int64_t a = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) +
286 (buf[pos + 2] << 8) + buf[pos + 3];
287 pos += 4;
288 int64_t b = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) +
289 (buf[pos + 2] << 8) + buf[pos + 3];
290 pos += 4;
291 p[element] = (a << 32) + (b & 0xffffffff);
292 }
293
294 return total_size;
295}
296
297static inline int __int64_t_clone_array(const int64_t *p, int64_t *q,
298 int elements) {
299 memcpy(q, p, elements * sizeof(int64_t));
300 return 0;
301}
302
303/**
304 * FLOAT
305 */
306#define __float_hash_recursive(p) 0
307#define __float_decode_array_cleanup(p, sz) \
308 {}
309#define float_encoded_size(p) (sizeof(int64_t) + sizeof(float))
310
311static inline int __float_encoded_array_size(const float *p, int elements) {
312 (void)p;
313 return sizeof(float) * elements;
314}
315
316static inline int __float_encode_array(void *_buf, int offset, int maxlen,
317 const float *p, int elements) {
318 return __int32_t_encode_array(_buf, offset, maxlen, (int32_t *)p, elements);
319}
320
321static inline int __float_decode_array(const void *_buf, int offset, int maxlen,
322 float *p, int elements) {
323 return __int32_t_decode_array(_buf, offset, maxlen, (int32_t *)p, elements);
324}
325
326static inline int __float_clone_array(const float *p, float *q, int elements) {
327 memcpy(q, p, elements * sizeof(float));
328 return 0;
329}
330
331/**
332 * DOUBLE
333 */
334#define __double_hash_recursive(p) 0
335#define __double_decode_array_cleanup(p, sz) \
336 {}
337#define double_encoded_size(p) (sizeof(int64_t) + sizeof(double))
338
339static inline int __double_encoded_array_size(const double *p, int elements) {
340 (void)p;
341 return sizeof(double) * elements;
342}
343
344static inline int __double_encode_array(void *_buf, int offset, int maxlen,
345 const double *p, int elements) {
346 return __int64_t_encode_array(_buf, offset, maxlen, (int64_t *)p, elements);
347}
348
349static inline int __double_decode_array(const void *_buf, int offset,
350 int maxlen, double *p, int elements) {
351 return __int64_t_decode_array(_buf, offset, maxlen, (int64_t *)p, elements);
352}
353
354static inline int __double_clone_array(const double *p, double *q,
355 int elements) {
356 memcpy(q, p, elements * sizeof(double));
357 return 0;
358}
359
360/**
361 * STRING
362 */
363#define __string_hash_recursive(p) 0
364
365static inline int __string_decode_array_cleanup(char **s, int elements) {
366 int element;
367 for (element = 0; element < elements; element++)
368 free(s[element]);
369 return 0;
370}
371
372static inline int __string_encoded_array_size(char *const *s, int elements) {
373 int size = 0;
374 int element;
375 for (element = 0; element < elements; element++)
376 size += 4 + strlen(s[element]) + 1;
377
378 return size;
379}
380
381static inline int __string_encoded_size(char *const *s) {
382 return sizeof(int64_t) + __string_encoded_array_size(s, 1);
383}
384
385static inline int __string_encode_array(void *_buf, int offset, int maxlen,
386 char *const *p, int elements) {
387 int pos = 0, thislen;
388 int element;
389
390 for (element = 0; element < elements; element++) {
391 int length = strlen(p[element]) + 1; // length includes \0
392
393 thislen = __int32_t_encode_array(_buf, offset + pos, maxlen - pos,
394 &length, 1);
395 if (thislen < 0)
396 return thislen;
397 else
398 pos += thislen;
399
400 thislen = __int8_t_encode_array(_buf, offset + pos, maxlen - pos,
401 (int8_t *)p[element], length);
402 if (thislen < 0)
403 return thislen;
404 else
405 pos += thislen;
406 }
407
408 return pos;
409}
410
411static inline int __string_decode_array(const void *_buf, int offset,
412 int maxlen, char **p, int elements) {
413 int pos = 0, thislen;
414 int element;
415
416 for (element = 0; element < elements; element++) {
417 int length;
418
419 // read length including \0
420 thislen = __int32_t_decode_array(_buf, offset + pos, maxlen - pos,
421 &length, 1);
422 if (thislen < 0)
423 return thislen;
424 else
425 pos += thislen;
426
427 p[element] = (char *)malloc(length);
428 thislen = __int8_t_decode_array(_buf, offset + pos, maxlen - pos,
429 (int8_t *)p[element], length);
430 if (thislen < 0)
431 return thislen;
432 else
433 pos += thislen;
434 }
435
436 return pos;
437}
438
439static inline int __string_clone_array(char *const *p, char **q, int elements) {
440 int element;
441 for (element = 0; element < elements; element++) {
442 // because strdup is not C99
443 size_t len = strlen(p[element]) + 1;
444 q[element] = (char *)malloc(len);
445 memcpy(q[element], p[element], len);
446 }
447 return 0;
448}
449
450static inline void *lcm_malloc(size_t sz) {
451 if (sz)
452 return malloc(sz);
453 return NULL;
454}
455
456#ifdef __cplusplus
457}
458#endif
459
460#endif
diff --git a/lcmtype/lcmtype.h b/lcmtype/lcmtype.h
new file mode 100644
index 0000000..a4fe704
--- /dev/null
+++ b/lcmtype/lcmtype.h
@@ -0,0 +1,14 @@
1#ifndef _LCMTYPE_H
2#define _LCMTYPE_H
3
4#ifdef __cplusplus
5extern "C" {
6#endif
7
8#include "lcm.h"
9#include "lcm_coretypes.h"
10#ifdef __cplusplus
11}
12#endif //__cplusplus
13
14#endif
diff --git a/lcmtype/lcmtype.mk b/lcmtype/lcmtype.mk
new file mode 100644
index 0000000..fc03109
--- /dev/null
+++ b/lcmtype/lcmtype.mk
@@ -0,0 +1,22 @@
1################################################################################
2# lcmtype.mk
3################################################################################
4# set compail dir
5MOD_DIR_NAME = lcmtype_dir
6MOD_SRC_DIR = $(KERNEL_TOP_DIR)/$(MOD_DIR_NAME)
7MOD_OBJ_DIR = ./project/$(MOD_DIR_NAME)
8
9# get files table
10ROBOT_KERNEL_OBJ += $(addprefix $(MOD_OBJ_DIR)/,$(subst .c,.o,$(wildcard $(MOD_SRC_DIR)/*.c)))
11ROBOT_KERNEL_FILE_DIR += $(wildcard $(MOD_SRC_DIR)/*.*~)
12
13# compail
14$(MOD_OBJ_DIR)/%.o: $(MOD_SRC_DIR)/%.c
15 @echo 'Compail... $<'
16 $(CC) -c $< -o $@ $(C_OPTIMIZATION) $(C_FLAG) $(USR_DEF)
17 @echo ' '
18
19#$(MOD_OBJ_DIR)/%.o: $(MOD_SRC_DIR)/%.c
20# @echo 'Compail... $<'
21# $(CC) -c $< -o $@ $(C_OPTIMIZATION) $(C_FLAG) $(USR_DEF)
22# @echo ' '
diff --git a/path/path_ctrl.lcm b/lcmtype/path_ctrl.lcm
index 502e342..502e342 100644
--- a/path/path_ctrl.lcm
+++ b/lcmtype/path_ctrl.lcm
diff --git a/path/path_ctrl_t.c b/lcmtype/path_ctrl_t.c
index 2298a56..2298a56 100644
--- a/path/path_ctrl_t.c
+++ b/lcmtype/path_ctrl_t.c
diff --git a/path/path_ctrl_t.h b/lcmtype/path_ctrl_t.h
index 3e629eb..052ccbc 100644
--- a/path/path_ctrl_t.h
+++ b/lcmtype/path_ctrl_t.h
@@ -5,8 +5,7 @@
5 5
6#include <stdint.h> 6#include <stdint.h>
7#include <stdlib.h> 7#include <stdlib.h>
8#include <lcm/lcm_coretypes.h> 8#include "lcmtype.h"
9#include <lcm/lcm.h>
10 9
11#ifndef _path_ctrl_t_h 10#ifndef _path_ctrl_t_h
12#define _path_ctrl_t_h 11#define _path_ctrl_t_h
diff --git a/lcmtype/path_t.cpp b/lcmtype/path_t.cpp
new file mode 100644
index 0000000..d6fde92
--- /dev/null
+++ b/lcmtype/path_t.cpp
@@ -0,0 +1,297 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "path_t.h"
8#include <string.h>
9
10static int __path_t_hash_computed;
11static int64_t __path_t_hash;
12
13int64_t __path_t_hash_recursive(const __lcm_hash_ptr *p) {
14 const __lcm_hash_ptr *fp;
15 for (fp = p; fp != NULL; fp = fp->parent)
16 if (fp->v == __path_t_get_hash)
17 return 0;
18
19 const __lcm_hash_ptr cp = {p, (void *)__path_t_get_hash};
20 (void)cp;
21
22 int64_t hash = 0xecdb2a054c696658LL + __int64_t_hash_recursive(&cp) +
23 __int16_t_hash_recursive(&cp) + __double_hash_recursive(&cp);
24
25 return (hash << 1) + ((hash >> 63) & 1);
26}
27
28int64_t __path_t_get_hash(void) {
29 if (!__path_t_hash_computed) {
30 __path_t_hash = __path_t_hash_recursive(NULL);
31 __path_t_hash_computed = 1;
32 }
33
34 return __path_t_hash;
35}
36
37int __path_t_encode_array(void *buf, int offset, int maxlen, const path_t *p,
38 int elements) {
39 int pos = 0, thislen, element;
40
41 for (element = 0; element < elements; element++) {
42
43 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
44 &(p[element].utime), 1);
45 if (thislen < 0)
46 return thislen;
47 else
48 pos += thislen;
49
50 thislen = __int16_t_encode_array(buf, offset + pos, maxlen - pos,
51 &(p[element].length), 1);
52 if (thislen < 0)
53 return thislen;
54 else
55 pos += thislen;
56
57 {
58 int a;
59 for (a = 0; a < p[element].length; a++) {
60 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
61 p[element].xyr[a], 3);
62 if (thislen < 0)
63 return thislen;
64 else
65 pos += thislen;
66 }
67 }
68 }
69 return pos;
70}
71
72int path_t_encode(void *buf, int offset, int maxlen, const path_t *p) {
73 int pos = 0, thislen;
74 int64_t hash = __path_t_get_hash();
75
76 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
77 if (thislen < 0)
78 return thislen;
79 else
80 pos += thislen;
81
82 thislen = __path_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
83 if (thislen < 0)
84 return thislen;
85 else
86 pos += thislen;
87
88 return pos;
89}
90
91int __path_t_encoded_array_size(const path_t *p, int elements) {
92 int size = 0, element;
93 for (element = 0; element < elements; element++) {
94
95 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
96
97 size += __int16_t_encoded_array_size(&(p[element].length), 1);
98
99 {
100 int a;
101 for (a = 0; a < p[element].length; a++) {
102 size += __double_encoded_array_size(p[element].xyr[a], 3);
103 }
104 }
105 }
106 return size;
107}
108
109int path_t_encoded_size(const path_t *p) {
110 return 8 + __path_t_encoded_array_size(p, 1);
111}
112
113int __path_t_decode_array(const void *buf, int offset, int maxlen, path_t *p,
114 int elements) {
115 int pos = 0, thislen, element;
116
117 for (element = 0; element < elements; element++) {
118
119 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
120 &(p[element].utime), 1);
121 if (thislen < 0)
122 return thislen;
123 else
124 pos += thislen;
125
126 thislen = __int16_t_decode_array(buf, offset + pos, maxlen - pos,
127 &(p[element].length), 1);
128 if (thislen < 0)
129 return thislen;
130 else
131 pos += thislen;
132
133 p[element].xyr =
134 (double **)lcm_malloc(sizeof(double *) * p[element].length);
135 {
136 int a;
137 for (a = 0; a < p[element].length; a++) {
138 p[element].xyr[a] = (double *)lcm_malloc(sizeof(double) * 3);
139 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
140 p[element].xyr[a], 3);
141 if (thislen < 0)
142 return thislen;
143 else
144 pos += thislen;
145 }
146 }
147 }
148 return pos;
149}
150
151int __path_t_decode_array_cleanup(path_t *p, int elements) {
152 int element;
153 for (element = 0; element < elements; element++) {
154
155 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
156
157 __int16_t_decode_array_cleanup(&(p[element].length), 1);
158
159 {
160 int a;
161 for (a = 0; a < p[element].length; a++) {
162 __double_decode_array_cleanup(p[element].xyr[a], 3);
163 if (p[element].xyr[a])
164 free(p[element].xyr[a]);
165 }
166 }
167 if (p[element].xyr)
168 free(p[element].xyr);
169 }
170 return 0;
171}
172
173int path_t_decode(const void *buf, int offset, int maxlen, path_t *p) {
174 int pos = 0, thislen;
175 int64_t hash = __path_t_get_hash();
176
177 int64_t this_hash;
178 thislen =
179 __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
180 if (thislen < 0)
181 return thislen;
182 else
183 pos += thislen;
184 if (this_hash != hash)
185 return -1;
186
187 thislen = __path_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
188 if (thislen < 0)
189 return thislen;
190 else
191 pos += thislen;
192
193 return pos;
194}
195
196int path_t_decode_cleanup(path_t *p) {
197 return __path_t_decode_array_cleanup(p, 1);
198}
199
200int __path_t_clone_array(const path_t *p, path_t *q, int elements) {
201 int element;
202 for (element = 0; element < elements; element++) {
203
204 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
205
206 __int16_t_clone_array(&(p[element].length), &(q[element].length), 1);
207
208 q[element].xyr =
209 (double **)lcm_malloc(sizeof(double *) * q[element].length);
210 {
211 int a;
212 for (a = 0; a < p[element].length; a++) {
213 q[element].xyr[a] = (double *)lcm_malloc(sizeof(double) * 3);
214 __double_clone_array(p[element].xyr[a], q[element].xyr[a], 3);
215 }
216 }
217 }
218 return 0;
219}
220
221path_t *path_t_copy(const path_t *p) {
222 path_t *q = (path_t *)malloc(sizeof(path_t));
223 __path_t_clone_array(p, q, 1);
224 return q;
225}
226
227void path_t_destroy(path_t *p) {
228 __path_t_decode_array_cleanup(p, 1);
229 free(p);
230}
231
232int path_t_publish(lcm_t *lc, const char *channel, const path_t *p) {
233 int max_data_size = path_t_encoded_size(p);
234 uint8_t *buf = (uint8_t *)malloc(max_data_size);
235 if (!buf)
236 return -1;
237 int data_size = path_t_encode(buf, 0, max_data_size, p);
238 if (data_size < 0) {
239 free(buf);
240 return data_size;
241 }
242 int status = lcm_publish(lc, channel, buf, data_size);
243 free(buf);
244 return status;
245}
246
247struct _path_t_subscription_t {
248 path_t_handler_t user_handler;
249 void *userdata;
250 lcm_subscription_t *lc_h;
251};
252static void path_t_handler_stub(const lcm_recv_buf_t *rbuf, const char *channel,
253 void *userdata) {
254 int status;
255 path_t p;
256 memset(&p, 0, sizeof(path_t));
257 status = path_t_decode(rbuf->data, 0, rbuf->data_size, &p);
258 if (status < 0) {
259 fprintf(stderr, "error %d decoding path_t!!!\n", status);
260 return;
261 }
262
263 path_t_subscription_t *h = (path_t_subscription_t *)userdata;
264 h->user_handler(rbuf, channel, &p, h->userdata);
265
266 path_t_decode_cleanup(&p);
267}
268
269path_t_subscription_t *path_t_subscribe(lcm_t *lcm, const char *channel,
270 path_t_handler_t f, void *userdata) {
271 path_t_subscription_t *n =
272 (path_t_subscription_t *)malloc(sizeof(path_t_subscription_t));
273 n->user_handler = f;
274 n->userdata = userdata;
275 n->lc_h = lcm_subscribe(lcm, channel, path_t_handler_stub, n);
276 if (n->lc_h == NULL) {
277 fprintf(stderr, "couldn't reg path_t LCM handler!\n");
278 free(n);
279 return NULL;
280 }
281 return n;
282}
283
284int path_t_subscription_set_queue_capacity(path_t_subscription_t *subs,
285 int num_messages) {
286 return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages);
287}
288
289int path_t_unsubscribe(lcm_t *lcm, path_t_subscription_t *hid) {
290 int status = lcm_unsubscribe(lcm, hid->lc_h);
291 if (0 != status) {
292 fprintf(stderr, "couldn't unsubscribe path_t_handler %p!\n", hid);
293 return -1;
294 }
295 free(hid);
296 return 0;
297}
diff --git a/lcmtype/path_t.h b/lcmtype/path_t.h
new file mode 100644
index 0000000..a9956c9
--- /dev/null
+++ b/lcmtype/path_t.h
@@ -0,0 +1,60 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "lcmtype.h"
8#include <stdint.h>
9#include <stdlib.h>
10
11#ifndef _path_t_h
12#define _path_t_h
13
14#ifdef __cplusplus
15extern "C" {
16#endif
17
18typedef struct _path_t path_t;
19struct _path_t {
20 int64_t utime;
21 int16_t length;
22 double **xyr;
23};
24
25path_t *path_t_copy(const path_t *p);
26void path_t_destroy(path_t *p);
27
28typedef struct _path_t_subscription_t path_t_subscription_t;
29typedef void (*path_t_handler_t)(const lcm_recv_buf_t *rbuf,
30 const char *channel, const path_t *msg,
31 void *user);
32
33int path_t_publish(lcm_t *lcm, const char *channel, const path_t *p);
34path_t_subscription_t *path_t_subscribe(lcm_t *lcm, const char *channel,
35 path_t_handler_t f, void *userdata);
36int path_t_unsubscribe(lcm_t *lcm, path_t_subscription_t *hid);
37int path_t_subscription_set_queue_capacity(path_t_subscription_t *subs,
38 int num_messages);
39
40int path_t_encode(void *buf, int offset, int maxlen, const path_t *p);
41int path_t_decode(const void *buf, int offset, int maxlen, path_t *p);
42int path_t_decode_cleanup(path_t *p);
43int path_t_encoded_size(const path_t *p);
44
45// LCM support functions. Users should not call these
46int64_t __path_t_get_hash(void);
47int64_t __path_t_hash_recursive(const __lcm_hash_ptr *p);
48int __path_t_encode_array(void *buf, int offset, int maxlen, const path_t *p,
49 int elements);
50int __path_t_decode_array(const void *buf, int offset, int maxlen, path_t *p,
51 int elements);
52int __path_t_decode_array_cleanup(path_t *p, int elements);
53int __path_t_encoded_array_size(const path_t *p, int elements);
54int __path_t_clone_array(const path_t *p, path_t *q, int elements);
55
56#ifdef __cplusplus
57}
58#endif
59
60#endif
diff --git a/lcmtype/pose_t.cpp b/lcmtype/pose_t.cpp
new file mode 100644
index 0000000..ef71a92
--- /dev/null
+++ b/lcmtype/pose_t.cpp
@@ -0,0 +1,325 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "pose_t.h"
8#include <string.h>
9
10static int __pose_t_hash_computed;
11static int64_t __pose_t_hash;
12
13int64_t __pose_t_hash_recursive(const __lcm_hash_ptr *p) {
14 const __lcm_hash_ptr *fp;
15 for (fp = p; fp != NULL; fp = fp->parent)
16 if (fp->v == __pose_t_get_hash)
17 return 0;
18
19 const __lcm_hash_ptr cp = {p, (void *)__pose_t_get_hash};
20 (void)cp;
21
22 int64_t hash = 0x170b77d82958082fLL + __int64_t_hash_recursive(&cp) +
23 __double_hash_recursive(&cp) + __double_hash_recursive(&cp) +
24 __double_hash_recursive(&cp) + __double_hash_recursive(&cp) +
25 __double_hash_recursive(&cp);
26
27 return (hash << 1) + ((hash >> 63) & 1);
28}
29
30int64_t __pose_t_get_hash(void) {
31 if (!__pose_t_hash_computed) {
32 __pose_t_hash = __pose_t_hash_recursive(NULL);
33 __pose_t_hash_computed = 1;
34 }
35
36 return __pose_t_hash;
37}
38
39int __pose_t_encode_array(void *buf, int offset, int maxlen, const pose_t *p,
40 int elements) {
41 int pos = 0, thislen, element;
42
43 for (element = 0; element < elements; element++) {
44
45 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
46 &(p[element].utime), 1);
47 if (thislen < 0)
48 return thislen;
49 else
50 pos += thislen;
51
52 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
53 p[element].pos, 3);
54 if (thislen < 0)
55 return thislen;
56 else
57 pos += thislen;
58
59 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
60 p[element].vel, 3);
61 if (thislen < 0)
62 return thislen;
63 else
64 pos += thislen;
65
66 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
67 p[element].orientation, 4);
68 if (thislen < 0)
69 return thislen;
70 else
71 pos += thislen;
72
73 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
74 p[element].rotation_rate, 3);
75 if (thislen < 0)
76 return thislen;
77 else
78 pos += thislen;
79
80 thislen = __double_encode_array(buf, offset + pos, maxlen - pos,
81 p[element].accel, 3);
82 if (thislen < 0)
83 return thislen;
84 else
85 pos += thislen;
86 }
87 return pos;
88}
89
90int pose_t_encode(void *buf, int offset, int maxlen, const pose_t *p) {
91 int pos = 0, thislen;
92 int64_t hash = __pose_t_get_hash();
93
94 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
95 if (thislen < 0)
96 return thislen;
97 else
98 pos += thislen;
99
100 thislen = __pose_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
101 if (thislen < 0)
102 return thislen;
103 else
104 pos += thislen;
105
106 return pos;
107}
108
109int __pose_t_encoded_array_size(const pose_t *p, int elements) {
110 int size = 0, element;
111 for (element = 0; element < elements; element++) {
112
113 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
114
115 size += __double_encoded_array_size(p[element].pos, 3);
116
117 size += __double_encoded_array_size(p[element].vel, 3);
118
119 size += __double_encoded_array_size(p[element].orientation, 4);
120
121 size += __double_encoded_array_size(p[element].rotation_rate, 3);
122
123 size += __double_encoded_array_size(p[element].accel, 3);
124 }
125 return size;
126}
127
128int pose_t_encoded_size(const pose_t *p) {
129 return 8 + __pose_t_encoded_array_size(p, 1);
130}
131
132int __pose_t_decode_array(const void *buf, int offset, int maxlen, pose_t *p,
133 int elements) {
134 int pos = 0, thislen, element;
135
136 for (element = 0; element < elements; element++) {
137
138 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
139 &(p[element].utime), 1);
140 if (thislen < 0)
141 return thislen;
142 else
143 pos += thislen;
144
145 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
146 p[element].pos, 3);
147 if (thislen < 0)
148 return thislen;
149 else
150 pos += thislen;
151
152 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
153 p[element].vel, 3);
154 if (thislen < 0)
155 return thislen;
156 else
157 pos += thislen;
158
159 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
160 p[element].orientation, 4);
161 if (thislen < 0)
162 return thislen;
163 else
164 pos += thislen;
165
166 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
167 p[element].rotation_rate, 3);
168 if (thislen < 0)
169 return thislen;
170 else
171 pos += thislen;
172
173 thislen = __double_decode_array(buf, offset + pos, maxlen - pos,
174 p[element].accel, 3);
175 if (thislen < 0)
176 return thislen;
177 else
178 pos += thislen;
179 }
180 return pos;
181}
182
183int __pose_t_decode_array_cleanup(pose_t *p, int elements) {
184 int element;
185 for (element = 0; element < elements; element++) {
186
187 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
188
189 __double_decode_array_cleanup(p[element].pos, 3);
190
191 __double_decode_array_cleanup(p[element].vel, 3);
192
193 __double_decode_array_cleanup(p[element].orientation, 4);
194
195 __double_decode_array_cleanup(p[element].rotation_rate, 3);
196
197 __double_decode_array_cleanup(p[element].accel, 3);
198 }
199 return 0;
200}
201
202int pose_t_decode(const void *buf, int offset, int maxlen, pose_t *p) {
203 int pos = 0, thislen;
204 int64_t hash = __pose_t_get_hash();
205
206 int64_t this_hash;
207 thislen =
208 __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
209 if (thislen < 0)
210 return thislen;
211 else
212 pos += thislen;
213 if (this_hash != hash)
214 return -1;
215
216 thislen = __pose_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
217 if (thislen < 0)
218 return thislen;
219 else
220 pos += thislen;
221
222 return pos;
223}
224
225int pose_t_decode_cleanup(pose_t *p) {
226 return __pose_t_decode_array_cleanup(p, 1);
227}
228
229int __pose_t_clone_array(const pose_t *p, pose_t *q, int elements) {
230 int element;
231 for (element = 0; element < elements; element++) {
232
233 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
234
235 __double_clone_array(p[element].pos, q[element].pos, 3);
236
237 __double_clone_array(p[element].vel, q[element].vel, 3);
238
239 __double_clone_array(p[element].orientation, q[element].orientation, 4);
240
241 __double_clone_array(p[element].rotation_rate, q[element].rotation_rate,
242 3);
243
244 __double_clone_array(p[element].accel, q[element].accel, 3);
245 }
246 return 0;
247}
248
249pose_t *pose_t_copy(const pose_t *p) {
250 pose_t *q = (pose_t *)malloc(sizeof(pose_t));
251 __pose_t_clone_array(p, q, 1);
252 return q;
253}
254
255void pose_t_destroy(pose_t *p) {
256 __pose_t_decode_array_cleanup(p, 1);
257 free(p);
258}
259
260int pose_t_publish(lcm_t *lc, const char *channel, const pose_t *p) {
261 int max_data_size = pose_t_encoded_size(p);
262 uint8_t *buf = (uint8_t *)malloc(max_data_size);
263 if (!buf)
264 return -1;
265 int data_size = pose_t_encode(buf, 0, max_data_size, p);
266 if (data_size < 0) {
267 free(buf);
268 return data_size;
269 }
270 int status = lcm_publish(lc, channel, buf, data_size);
271 free(buf);
272 return status;
273}
274
275struct _pose_t_subscription_t {
276 pose_t_handler_t user_handler;
277 void *userdata;
278 lcm_subscription_t *lc_h;
279};
280static void pose_t_handler_stub(const lcm_recv_buf_t *rbuf, const char *channel,
281 void *userdata) {
282 int status;
283 pose_t p;
284 memset(&p, 0, sizeof(pose_t));
285 status = pose_t_decode(rbuf->data, 0, rbuf->data_size, &p);
286 if (status < 0) {
287 fprintf(stderr, "error %d decoding pose_t!!!\n", status);
288 return;
289 }
290
291 pose_t_subscription_t *h = (pose_t_subscription_t *)userdata;
292 h->user_handler(rbuf, channel, &p, h->userdata);
293
294 pose_t_decode_cleanup(&p);
295}
296
297pose_t_subscription_t *pose_t_subscribe(lcm_t *lcm, const char *channel,
298 pose_t_handler_t f, void *userdata) {
299 pose_t_subscription_t *n =
300 (pose_t_subscription_t *)malloc(sizeof(pose_t_subscription_t));
301 n->user_handler = f;
302 n->userdata = userdata;
303 n->lc_h = lcm_subscribe(lcm, channel, pose_t_handler_stub, n);
304 if (n->lc_h == NULL) {
305 fprintf(stderr, "couldn't reg pose_t LCM handler!\n");
306 free(n);
307 return NULL;
308 }
309 return n;
310}
311
312int pose_t_subscription_set_queue_capacity(pose_t_subscription_t *subs,
313 int num_messages) {
314 return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages);
315}
316
317int pose_t_unsubscribe(lcm_t *lcm, pose_t_subscription_t *hid) {
318 int status = lcm_unsubscribe(lcm, hid->lc_h);
319 if (0 != status) {
320 fprintf(stderr, "couldn't unsubscribe pose_t_handler %p!\n", hid);
321 return -1;
322 }
323 free(hid);
324 return 0;
325}
diff --git a/lcmtype/pose_t.h b/lcmtype/pose_t.h
new file mode 100644
index 0000000..621cd5c
--- /dev/null
+++ b/lcmtype/pose_t.h
@@ -0,0 +1,63 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "lcmtype.h"
8#include <stdint.h>
9#include <stdlib.h>
10
11#ifndef _pose_t_h
12#define _pose_t_h
13
14#ifdef __cplusplus
15extern "C" {
16#endif
17
18typedef struct _pose_t pose_t;
19struct _pose_t {
20 int64_t utime;
21 double pos[3];
22 double vel[3];
23 double orientation[4];
24 double rotation_rate[3];
25 double accel[3];
26};
27
28pose_t *pose_t_copy(const pose_t *p);
29void pose_t_destroy(pose_t *p);
30
31typedef struct _pose_t_subscription_t pose_t_subscription_t;
32typedef void (*pose_t_handler_t)(const lcm_recv_buf_t *rbuf,
33 const char *channel, const pose_t *msg,
34 void *user);
35
36int pose_t_publish(lcm_t *lcm, const char *channel, const pose_t *p);
37pose_t_subscription_t *pose_t_subscribe(lcm_t *lcm, const char *channel,
38 pose_t_handler_t f, void *userdata);
39int pose_t_unsubscribe(lcm_t *lcm, pose_t_subscription_t *hid);
40int pose_t_subscription_set_queue_capacity(pose_t_subscription_t *subs,
41 int num_messages);
42
43int pose_t_encode(void *buf, int offset, int maxlen, const pose_t *p);
44int pose_t_decode(const void *buf, int offset, int maxlen, pose_t *p);
45int pose_t_decode_cleanup(pose_t *p);
46int pose_t_encoded_size(const pose_t *p);
47
48// LCM support functions. Users should not call these
49int64_t __pose_t_get_hash(void);
50int64_t __pose_t_hash_recursive(const __lcm_hash_ptr *p);
51int __pose_t_encode_array(void *buf, int offset, int maxlen, const pose_t *p,
52 int elements);
53int __pose_t_decode_array(const void *buf, int offset, int maxlen, pose_t *p,
54 int elements);
55int __pose_t_decode_array_cleanup(pose_t *p, int elements);
56int __pose_t_encoded_array_size(const pose_t *p, int elements);
57int __pose_t_clone_array(const pose_t *p, pose_t *q, int elements);
58
59#ifdef __cplusplus
60}
61#endif
62
63#endif
diff --git a/lcmtype/robot_control_t.cpp b/lcmtype/robot_control_t.cpp
new file mode 100644
index 0000000..d633435
--- /dev/null
+++ b/lcmtype/robot_control_t.cpp
@@ -0,0 +1,478 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "robot_control_t.h"
8#include <string.h>
9
10static int __robot_control_t_hash_computed;
11static int64_t __robot_control_t_hash;
12
13int64_t __robot_control_t_hash_recursive(const __lcm_hash_ptr *p) {
14 const __lcm_hash_ptr *fp;
15 for (fp = p; fp != NULL; fp = fp->parent)
16 if (fp->v == __robot_control_t_get_hash)
17 return 0;
18
19 const __lcm_hash_ptr cp = {p, (void *)__robot_control_t_get_hash};
20 (void)cp;
21
22 int64_t hash = 0x38f63251f9863f70LL + __int64_t_hash_recursive(&cp) +
23 __int8_t_hash_recursive(&cp) + __int8_t_hash_recursive(&cp) +
24 __int8_t_hash_recursive(&cp) + __double_hash_recursive(&cp) +
25 __int8_t_hash_recursive(&cp) + __int8_t_hash_recursive(&cp) +
26 __int8_t_hash_recursive(&cp) + __string_hash_recursive(&cp) +
27 __int64_t_hash_recursive(&cp) + __byte_hash_recursive(&cp);
28
29 return (hash << 1) + ((hash >> 63) & 1);
30}
31
32int64_t __robot_control_t_get_hash(void) {
33 if (!__robot_control_t_hash_computed) {
34 __robot_control_t_hash = __robot_control_t_hash_recursive(NULL);
35 __robot_control_t_hash_computed = 1;
36 }
37
38 return __robot_control_t_hash;
39}
40
41int __robot_control_t_encode_array(void *buf, int offset, int maxlen,
42 const robot_control_t *p, int elements) {
43 int pos = 0, thislen, element;
44
45 for (element = 0; element < elements; element++) {
46
47 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
48 &(p[element].utime), 1);
49 if (thislen < 0)
50 return thislen;
51 else
52 pos += thislen;
53
54 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
55 &(p[element].commandid), 1);
56 if (thislen < 0)
57 return thislen;
58 else
59 pos += thislen;
60
61 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
62 &(p[element].robotid), 1);
63 if (thislen < 0)
64 return thislen;
65 else
66 pos += thislen;
67
68 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
69 &(p[element].ndparams), 1);
70 if (thislen < 0)
71 return thislen;
72 else
73 pos += thislen;
74
75 thislen =
76 __double_encode_array(buf, offset + pos, maxlen - pos,
77 p[element].dparams, p[element].ndparams);
78 if (thislen < 0)
79 return thislen;
80 else
81 pos += thislen;
82
83 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
84 &(p[element].niparams), 1);
85 if (thislen < 0)
86 return thislen;
87 else
88 pos += thislen;
89
90 thislen =
91 __int8_t_encode_array(buf, offset + pos, maxlen - pos,
92 p[element].iparams, p[element].niparams);
93 if (thislen < 0)
94 return thislen;
95 else
96 pos += thislen;
97
98 thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos,
99 &(p[element].nsparams), 1);
100 if (thislen < 0)
101 return thislen;
102 else
103 pos += thislen;
104
105 thislen =
106 __string_encode_array(buf, offset + pos, maxlen - pos,
107 p[element].sparams, p[element].nsparams);
108 if (thislen < 0)
109 return thislen;
110 else
111 pos += thislen;
112
113 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos,
114 &(p[element].nbparams), 1);
115 if (thislen < 0)
116 return thislen;
117 else
118 pos += thislen;
119
120 thislen = __byte_encode_array(buf, offset + pos, maxlen - pos,
121 p[element].bparams, p[element].nbparams);
122 if (thislen < 0)
123 return thislen;
124 else
125 pos += thislen;
126 }
127 return pos;
128}
129
130int robot_control_t_encode(void *buf, int offset, int maxlen,
131 const robot_control_t *p) {
132 int pos = 0, thislen;
133 int64_t hash = __robot_control_t_get_hash();
134
135 thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
136 if (thislen < 0)
137 return thislen;
138 else
139 pos += thislen;
140
141 thislen =
142 __robot_control_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
143 if (thislen < 0)
144 return thislen;
145 else
146 pos += thislen;
147
148 return pos;
149}
150
151int __robot_control_t_encoded_array_size(const robot_control_t *p,
152 int elements) {
153 int size = 0, element;
154 for (element = 0; element < elements; element++) {
155
156 size += __int64_t_encoded_array_size(&(p[element].utime), 1);
157
158 size += __int8_t_encoded_array_size(&(p[element].commandid), 1);
159
160 size += __int8_t_encoded_array_size(&(p[element].robotid), 1);
161
162 size += __int8_t_encoded_array_size(&(p[element].ndparams), 1);
163
164 size += __double_encoded_array_size(p[element].dparams,
165 p[element].ndparams);
166
167 size += __int8_t_encoded_array_size(&(p[element].niparams), 1);
168
169 size += __int8_t_encoded_array_size(p[element].iparams,
170 p[element].niparams);
171
172 size += __int8_t_encoded_array_size(&(p[element].nsparams), 1);
173
174 size += __string_encoded_array_size(p[element].sparams,
175 p[element].nsparams);
176
177 size += __int64_t_encoded_array_size(&(p[element].nbparams), 1);
178
179 size +=
180 __byte_encoded_array_size(p[element].bparams, p[element].nbparams);
181 }
182 return size;
183}
184
185int robot_control_t_encoded_size(const robot_control_t *p) {
186 return 8 + __robot_control_t_encoded_array_size(p, 1);
187}
188
189int __robot_control_t_decode_array(const void *buf, int offset, int maxlen,
190 robot_control_t *p, int elements) {
191 int pos = 0, thislen, element;
192
193 for (element = 0; element < elements; element++) {
194
195 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
196 &(p[element].utime), 1);
197 if (thislen < 0)
198 return thislen;
199 else
200 pos += thislen;
201
202 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
203 &(p[element].commandid), 1);
204 if (thislen < 0)
205 return thislen;
206 else
207 pos += thislen;
208
209 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
210 &(p[element].robotid), 1);
211 if (thislen < 0)
212 return thislen;
213 else
214 pos += thislen;
215
216 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
217 &(p[element].ndparams), 1);
218 if (thislen < 0)
219 return thislen;
220 else
221 pos += thislen;
222
223 p[element].dparams =
224 (double *)lcm_malloc(sizeof(double) * p[element].ndparams);
225 thislen =
226 __double_decode_array(buf, offset + pos, maxlen - pos,
227 p[element].dparams, p[element].ndparams);
228 if (thislen < 0)
229 return thislen;
230 else
231 pos += thislen;
232
233 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
234 &(p[element].niparams), 1);
235 if (thislen < 0)
236 return thislen;
237 else
238 pos += thislen;
239
240 p[element].iparams =
241 (int8_t *)lcm_malloc(sizeof(int8_t) * p[element].niparams);
242 thislen =
243 __int8_t_decode_array(buf, offset + pos, maxlen - pos,
244 p[element].iparams, p[element].niparams);
245 if (thislen < 0)
246 return thislen;
247 else
248 pos += thislen;
249
250 thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos,
251 &(p[element].nsparams), 1);
252 if (thislen < 0)
253 return thislen;
254 else
255 pos += thislen;
256
257 p[element].sparams =
258 (char **)lcm_malloc(sizeof(char *) * p[element].nsparams);
259 thislen =
260 __string_decode_array(buf, offset + pos, maxlen - pos,
261 p[element].sparams, p[element].nsparams);
262 if (thislen < 0)
263 return thislen;
264 else
265 pos += thislen;
266
267 thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos,
268 &(p[element].nbparams), 1);
269 if (thislen < 0)
270 return thislen;
271 else
272 pos += thislen;
273
274 p[element].bparams =
275 (uint8_t *)lcm_malloc(sizeof(uint8_t) * p[element].nbparams);
276 thislen = __byte_decode_array(buf, offset + pos, maxlen - pos,
277 p[element].bparams, p[element].nbparams);
278 if (thislen < 0)
279 return thislen;
280 else
281 pos += thislen;
282 }
283 return pos;
284}
285
286int __robot_control_t_decode_array_cleanup(robot_control_t *p, int elements) {
287 int element;
288 for (element = 0; element < elements; element++) {
289
290 __int64_t_decode_array_cleanup(&(p[element].utime), 1);
291
292 __int8_t_decode_array_cleanup(&(p[element].commandid), 1);
293
294 __int8_t_decode_array_cleanup(&(p[element].robotid), 1);
295
296 __int8_t_decode_array_cleanup(&(p[element].ndparams), 1);
297
298 __double_decode_array_cleanup(p[element].dparams, p[element].ndparams);
299 if (p[element].dparams)
300 free(p[element].dparams);
301
302 __int8_t_decode_array_cleanup(&(p[element].niparams), 1);
303
304 __int8_t_decode_array_cleanup(p[element].iparams, p[element].niparams);
305 if (p[element].iparams)
306 free(p[element].iparams);
307
308 __int8_t_decode_array_cleanup(&(p[element].nsparams), 1);
309
310 __string_decode_array_cleanup(p[element].sparams, p[element].nsparams);
311 if (p[element].sparams)
312 free(p[element].sparams);
313
314 __int64_t_decode_array_cleanup(&(p[element].nbparams), 1);
315
316 __byte_decode_array_cleanup(p[element].bparams, p[element].nbparams);
317 if (p[element].bparams)
318 free(p[element].bparams);
319 }
320 return 0;
321}
322
323int robot_control_t_decode(const void *buf, int offset, int maxlen,
324 robot_control_t *p) {
325 int pos = 0, thislen;
326 int64_t hash = __robot_control_t_get_hash();
327
328 int64_t this_hash;
329 thislen =
330 __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
331 if (thislen < 0)
332 return thislen;
333 else
334 pos += thislen;
335 if (this_hash != hash)
336 return -1;
337
338 thislen =
339 __robot_control_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
340 if (thislen < 0)
341 return thislen;
342 else
343 pos += thislen;
344
345 return pos;
346}
347
348int robot_control_t_decode_cleanup(robot_control_t *p) {
349 return __robot_control_t_decode_array_cleanup(p, 1);
350}
351
352int __robot_control_t_clone_array(const robot_control_t *p, robot_control_t *q,
353 int elements) {
354 int element;
355 for (element = 0; element < elements; element++) {
356
357 __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1);
358
359 __int8_t_clone_array(&(p[element].commandid), &(q[element].commandid),
360 1);
361
362 __int8_t_clone_array(&(p[element].robotid), &(q[element].robotid), 1);
363
364 __int8_t_clone_array(&(p[element].ndparams), &(q[element].ndparams), 1);
365
366 q[element].dparams =
367 (double *)lcm_malloc(sizeof(double) * q[element].ndparams);
368 __double_clone_array(p[element].dparams, q[element].dparams,
369 p[element].ndparams);
370
371 __int8_t_clone_array(&(p[element].niparams), &(q[element].niparams), 1);
372
373 q[element].iparams =
374 (int8_t *)lcm_malloc(sizeof(int8_t) * q[element].niparams);
375 __int8_t_clone_array(p[element].iparams, q[element].iparams,
376 p[element].niparams);
377
378 __int8_t_clone_array(&(p[element].nsparams), &(q[element].nsparams), 1);
379
380 q[element].sparams =
381 (char **)lcm_malloc(sizeof(char *) * q[element].nsparams);
382 __string_clone_array(p[element].sparams, q[element].sparams,
383 p[element].nsparams);
384
385 __int64_t_clone_array(&(p[element].nbparams), &(q[element].nbparams),
386 1);
387
388 q[element].bparams =
389 (uint8_t *)lcm_malloc(sizeof(uint8_t) * q[element].nbparams);
390 __byte_clone_array(p[element].bparams, q[element].bparams,
391 p[element].nbparams);
392 }
393 return 0;
394}
395
396robot_control_t *robot_control_t_copy(const robot_control_t *p) {
397 robot_control_t *q = (robot_control_t *)malloc(sizeof(robot_control_t));
398 __robot_control_t_clone_array(p, q, 1);
399 return q;
400}
401
402void robot_control_t_destroy(robot_control_t *p) {
403 __robot_control_t_decode_array_cleanup(p, 1);
404 free(p);
405}
406
407int robot_control_t_publish(lcm_t *lc, const char *channel,
408 const robot_control_t *p) {
409 int max_data_size = robot_control_t_encoded_size(p);
410 uint8_t *buf = (uint8_t *)malloc(max_data_size);
411 if (!buf)
412 return -1;
413 int data_size = robot_control_t_encode(buf, 0, max_data_size, p);
414 if (data_size < 0) {
415 free(buf);
416 return data_size;
417 }
418 int status = lcm_publish(lc, channel, buf, data_size);
419 free(buf);
420 return status;
421}
422
423struct _robot_control_t_subscription_t {
424 robot_control_t_handler_t user_handler;
425 void *userdata;
426 lcm_subscription_t *lc_h;
427};
428static void robot_control_t_handler_stub(const lcm_recv_buf_t *rbuf,
429 const char *channel, void *userdata) {
430 int status;
431 robot_control_t p;
432 memset(&p, 0, sizeof(robot_control_t));
433 status = robot_control_t_decode(rbuf->data, 0, rbuf->data_size, &p);
434 if (status < 0) {
435 fprintf(stderr, "error %d decoding robot_control_t!!!\n", status);
436 return;
437 }
438
439 robot_control_t_subscription_t *h =
440 (robot_control_t_subscription_t *)userdata;
441 h->user_handler(rbuf, channel, &p, h->userdata);
442
443 robot_control_t_decode_cleanup(&p);
444}
445
446robot_control_t_subscription_t *
447robot_control_t_subscribe(lcm_t *lcm, const char *channel,
448 robot_control_t_handler_t f, void *userdata) {
449 robot_control_t_subscription_t *n =
450 (robot_control_t_subscription_t *)malloc(
451 sizeof(robot_control_t_subscription_t));
452 n->user_handler = f;
453 n->userdata = userdata;
454 n->lc_h = lcm_subscribe(lcm, channel, robot_control_t_handler_stub, n);
455 if (n->lc_h == NULL) {
456 fprintf(stderr, "couldn't reg robot_control_t LCM handler!\n");
457 free(n);
458 return NULL;
459 }
460 return n;
461}
462
463int robot_control_t_subscription_set_queue_capacity(
464 robot_control_t_subscription_t *subs, int num_messages) {
465 return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages);
466}
467
468int robot_control_t_unsubscribe(lcm_t *lcm,
469 robot_control_t_subscription_t *hid) {
470 int status = lcm_unsubscribe(lcm, hid->lc_h);
471 if (0 != status) {
472 fprintf(stderr, "couldn't unsubscribe robot_control_t_handler %p!\n",
473 hid);
474 return -1;
475 }
476 free(hid);
477 return 0;
478}
diff --git a/lcmtype/robot_control_t.h b/lcmtype/robot_control_t.h
new file mode 100644
index 0000000..d28de92
--- /dev/null
+++ b/lcmtype/robot_control_t.h
@@ -0,0 +1,76 @@
1/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY
2 * BY HAND!!
3 *
4 * Generated by lcm-gen
5 **/
6
7#include "lcmtype.h"
8#include <stdint.h>
9#include <stdlib.h>
10
11#ifndef _robot_control_t_h
12#define _robot_control_t_h
13
14#ifdef __cplusplus
15extern "C" {
16#endif
17
18typedef struct _robot_control_t robot_control_t;
19struct _robot_control_t {
20 int64_t utime;
21 int8_t commandid;
22 int8_t robotid;
23 int8_t ndparams;
24 double *dparams;
25 int8_t niparams;
26 int8_t *iparams;
27 int8_t nsparams;
28 char **sparams;
29 int64_t nbparams;
30 uint8_t *bparams;
31};
32
33robot_control_t *robot_control_t_copy(const robot_control_t *p);
34void robot_control_t_destroy(robot_control_t *p);
35
36typedef struct _robot_control_t_subscription_t robot_control_t_subscription_t;
37typedef void (*robot_control_t_handler_t)(const lcm_recv_buf_t *rbuf,
38 const char *channel,
39 const robot_control_t *msg,
40 void *user);
41
42int robot_control_t_publish(lcm_t *lcm, const char *channel,
43 const robot_control_t *p);
44robot_control_t_subscription_t *
45robot_control_t_subscribe(lcm_t *lcm, const char *channel,
46 robot_control_t_handler_t f, void *userdata);
47int robot_control_t_unsubscribe(lcm_t *lcm,
48 robot_control_t_subscription_t *hid);
49int robot_control_t_subscription_set_queue_capacity(
50 robot_control_t_subscription_t *subs, int num_messages);
51
52int robot_control_t_encode(void *buf, int offset, int maxlen,
53 const robot_control_t *p);
54int robot_control_t_decode(const void *buf, int offset, int maxlen,
55 robot_control_t *p);
56int robot_control_t_decode_cleanup(robot_control_t *p);
57int robot_control_t_encoded_size(const robot_control_t *p);
58
59// LCM support functions. Users should not call these
60int64_t __robot_control_t_get_hash(void);
61int64_t __robot_control_t_hash_recursive(const __lcm_hash_ptr *p);
62int __robot_control_t_encode_array(void *buf, int offset, int maxlen,
63 const robot_control_t *p, int elements);
64int __robot_control_t_decode_array(const void *buf, int offset, int maxlen,
65 robot_control_t *p, int elements);
66int __robot_control_t_decode_array_cleanup(robot_control_t *p, int elements);
67int __robot_control_t_encoded_array_size(const robot_control_t *p,
68 int elements);
69int __robot_control_t_clone_array(const robot_control_t *p, robot_control_t *q,
70 int elements);
71
72#ifdef __cplusplus
73}
74#endif
75
76#endif
diff --git a/lib/aarch64/liblcm.so b/lib/aarch64/liblcm.so
new file mode 100644
index 0000000..98bb954
--- /dev/null
+++ b/lib/aarch64/liblcm.so
Binary files differ
diff --git a/lib/liblcm.so b/lib/x86/liblcm.so
index cb18db8..cb18db8 100755
--- a/lib/liblcm.so
+++ b/lib/x86/liblcm.so
Binary files differ
diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt
new file mode 100644
index 0000000..55415f3
--- /dev/null
+++ b/serial/CMakeLists.txt
@@ -0,0 +1,7 @@
1cmake_minimum_required(VERSION 3.10)
2project(serial)
3
4add_executable(serial serial.c ${CMAKE_SOURCE_DIR}/lcmtype/path_ctrl_t.c)
5
6# 添加链接库
7target_link_libraries(serial lcm) \ No newline at end of file
diff --git a/serial.c b/serial/serial.c
index 6b4eb72..db45fac 100644
--- a/serial.c
+++ b/serial/serial.c
@@ -1,4 +1,4 @@
1#include "path/path_ctrl_t.h" 1#include "path_ctrl_t.h"
2#include <errno.h> 2#include <errno.h>
3#include <fcntl.h> 3#include <fcntl.h>
4#include <stdbool.h> 4#include <stdbool.h>
diff --git a/udp2lcm.c b/udp2lcm.c
deleted file mode 100644
index c328a96..0000000
--- a/udp2lcm.c
+++ /dev/null
@@ -1,198 +0,0 @@
1// Encoded in UTF-8
2
3/*
4 * struct path_ctrl_t
5 * {
6 * int8_t cmd;
7 * int8_t speed;
8 * }
9 * 这是与轮控模块通信的数据结构，speed为0-100表示速度
10 * cmd为0停，1前，2左，3右
11 */
12#include "path/path_ctrl_t.h"
13#include <arpa/inet.h>
14#include <errno.h>
15#include <fcntl.h>
16#include <netinet/in.h>
17#include <poll.h>
18#include <pthread.h>
19#include <stdbool.h>
20#include <stdio.h>
21#include <stdlib.h>
22#include <string.h>
23#include <sys/socket.h>
24#include <termios.h>
25#include <unistd.h>
26
27#define MAX_BUFFER_SIZE 1024
28#define PORT 5001
29
30pthread_t udpRecv, udpSend;
31int8_t heartBeat[9] = {0}; // 用于存储需要发送的心跳信息
32// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改
33// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致
34struct sockaddr_in clientAddr;
35char clientIP[20] = "";
36lcm_t *path_ctrl_lcm;
37
38bool LCMInit();
39void udpSendHandler();
40void udpRecvHandler();
41void parseCmd(const char *buffer, int bytesReceived);
42
43int main() {
44 if (!LCMInit()) {
45 fprintf(stderr, "Error: Failed to initialize LCM\n");
46 return -1;
47 }
48 // 开启udp接收线程
49 pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
50 while (true) {
51 pause();
52 }
53 return 0;
54}
55
56bool LCMInit() {
57 // TODO: 在这里同意初始化各个LCM变量
58 // ATTENTION:
59 // 是否在这里订阅各个通道、各个通道需要在什么地方订阅，需要慎重考虑
60 if ((path_ctrl_lcm = lcm_create(NULL)) == NULL) {
61 fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
62 return false;
63 }
64
65 return true;
66}
67
68void udpRecvHandler() {
69 int socketfd = -1;
70 char buffer[MAX_BUFFER_SIZE];
71 struct sockaddr_in serverAddr;
72 socklen_t addrLen = sizeof(serverAddr);
73 int bytesReceived;
74
75 int retval;
76 struct pollfd fds;
77
78 printf("udpRecvHandler\n");
79 if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
80 fprintf(stderr, "Error: Failed to create socket\n");
81 return;
82 }
83 memset((char *)&serverAddr, 0, sizeof(serverAddr));
84 memset(buffer, 0, sizeof(buffer));
85 serverAddr.sin_family = AF_INET;
86 serverAddr.sin_port = htons(PORT);
87 serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
88
89 if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
90 -1) {
91 fprintf(stderr, "Error: Bind failed\n");
92 close(socketfd);
93 return;
94 }
95
96 bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
97 (struct sockaddr *)&clientAddr, &addrLen);
98 if (bytesReceived == -1) {
99 fprintf(stderr, "Error: Failed to receive data\n");
100 close(socketfd);
101 return;
102 }
103 printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
104 buffer);
105 strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
106
107 // 在接收到第一条消息之后，开启udp发送线程，用于发送心跳信息
108 if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) {
109 fprintf(stderr, "Error creating udpSend thread\n");
110 close(socketfd);
111 exit(-1);
112 }
113
114 fds.fd = socketfd;
115 fds.events = POLLIN;
116
117 while (true) {
118 retval = poll(&fds, 1, 3000);
119 if (retval == -1) {
120 fprintf(stderr, "Error: select failed\n");
121 break;
122 } else if (retval == 0) {
123 printf("No data within three seconds.\n");
124 // out of 3s, stop the wheel
125 path_ctrl_t path = {0, 0};
126 path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path);
127 } else {
128 bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
129 (struct sockaddr *)&clientAddr, &addrLen);
130 if (bytesReceived == -1) {
131 fprintf(stderr, "Error: Failed to receive data\n");
132 continue;
133 }
134 printf("Massage from client %s: ", clientIP);
135 for (int i = 0; i < bytesReceived; i++) {
136 printf("%02x ", buffer[i]);
137 }
138 printf("\n");
139 parseCmd(buffer, bytesReceived);
140 }
141 }
142}
143
144void udpSendHandler() {
145 // 创建UDP套接字
146 int sockfd = -1;
147 struct sockaddr_in serverAddr;
148 if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
149 fprintf(stderr, "Error: Failed to create socket\n");
150 return;
151 }
152
153 memset((char *)&serverAddr, 0, sizeof(serverAddr));
154 serverAddr.sin_family = AF_INET; // 设置地址族为IPv4
155 serverAddr.sin_port = htons(PORT); // 设置端口号
156 if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
157 fprintf(stderr, "Error: Failed to convert IP address\n");
158 close(sockfd);
159 return;
160 }
161
162 while (true) {
163 sendto(sockfd, (const char *)heartBeat, 9, 0,
164 (struct sockaddr *)&serverAddr, sizeof(serverAddr));
165 sleep(1);
166 }
167}
168
169void parseCmd(const char *buffer, int bytesReceived) {
170 // TODO: 完成本函数的内容
171 if (buffer == NULL) {
172 fprintf(stderr, "Error: Invalid message\n");
173 return;
174 }
175
176 if (buffer[0] == 0) {
177 // 手机端用来与udp2lcm服务器建立连接的初始化消息
178 fprintf(stderr, "Why Init cmd here?\n");
179 } else if (buffer[0] == 1) {
180 // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
181 path_ctrl_t path;
182 path.cmd = buffer[1];
183 path.speed = buffer[2];
184 path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path);
185 } else if (buffer[0] == 2) {
186 /*
187 * 结束建图，主要有以下几个任务：
188 * - 向轮控模块发送停止命令
189 * - 通知算法模块结束建图（FIXME: 是否接受算法模块的回应？）
190 * - 拉起http服务器进程
191 * - 向手机端通知建图完成
192 * -
193 * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
194 */
195 } else if (buffer[0] == 3) {
196 // 手机发来的终点坐标，转发给算法模块
197 }
198} \ No newline at end of file
diff --git a/udp2lcm/CMakeLists.txt b/udp2lcm/CMakeLists.txt
new file mode 100644
index 0000000..aaf54db
--- /dev/null
+++ b/udp2lcm/CMakeLists.txt
@@ -0,0 +1,10 @@
1cmake_minimum_required(VERSION 3.10)
2project(udp2lcm)
3
4# 设置源文件为本文件夹下的所有.c .cpp文件以及lcmtype文件夹下的所有.c .cpp文件
5aux_source_directory(. DIR_SRCS)
6aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS)
7
8# 生成可执行文件
9add_executable(udp2lcm ${DIR_SRCS})
10target_link_libraries(udp2lcm lcm) \ No newline at end of file
diff --git a/udp2lcm/robotSet.c b/udp2lcm/robotSet.c
new file mode 100644
index 0000000..20b18a5
--- /dev/null
+++ b/udp2lcm/robotSet.c
@@ -0,0 +1,60 @@
1#include "udp2lcm.h"
2
3void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
4 int8_t commandid, int8_t robotid, int8_t ndparams,
5 int8_t niparams, int8_t nsparams, int64_t nbparams) {
6 if (robotCtrlData == NULL) {
7 fprintf(stderr, "Error: Invalid robot control data\n");
8 return;
9 }
10 robotCtrlData->utime = utime;
11 robotCtrlData->commandid = commandid;
12 robotCtrlData->robotid = robotid;
13 robotCtrlData->ndparams = ndparams;
14 robotCtrlData->dparams = NULL;
15 robotCtrlData->niparams = niparams;
16 robotCtrlData->iparams = NULL;
17 robotCtrlData->nsparams = nsparams;
18 robotCtrlData->sparams = NULL;
19 robotCtrlData->nbparams = nbparams;
20 robotCtrlData->bparams = NULL;
21 if (ndparams > 0) {
22 robotCtrlData->dparams = (double *)malloc(ndparams * sizeof(double));
23 memset(robotCtrlData->dparams, 0, ndparams * sizeof(double));
24 }
25 if (niparams > 0) {
26 robotCtrlData->iparams = (int8_t *)malloc(niparams * sizeof(int8_t));
27 memset(robotCtrlData->iparams, 0, niparams * sizeof(int8_t));
28 }
29 if (nsparams > 0) {
30 robotCtrlData->sparams = (char **)malloc(nsparams * sizeof(char *));
31 memset(robotCtrlData->sparams, 0, nsparams * sizeof(char *));
32 }
33 if (nbparams > 0) {
34 robotCtrlData->bparams = (uint8_t *)malloc(nbparams * sizeof(uint8_t));
35 memset(robotCtrlData->bparams, 0, nbparams * sizeof(uint8_t));
36 }
37}
38
39void freeRobotCtrl(robot_control_t *robotCtrlData) {
40 if (robotCtrlData == NULL) {
41 fprintf(stderr, "Error: Invalid robot control data\n");
42 return;
43 }
44 if (robotCtrlData->dparams != NULL) {
45 free(robotCtrlData->dparams);
46 robotCtrlData->dparams = NULL;
47 }
48 if (robotCtrlData->iparams != NULL) {
49 free(robotCtrlData->iparams);
50 robotCtrlData->iparams = NULL;
51 }
52 if (robotCtrlData->sparams != NULL) {
53 free(robotCtrlData->sparams);
54 robotCtrlData->sparams = NULL;
55 }
56 if (robotCtrlData->bparams != NULL) {
57 free(robotCtrlData->bparams);
58 robotCtrlData->bparams = NULL;
59 }
60} \ No newline at end of file
diff --git a/udp2lcm/udp.c b/udp2lcm/udp.c
new file mode 100644
index 0000000..6c4bd4a
--- /dev/null
+++ b/udp2lcm/udp.c
@@ -0,0 +1,127 @@
1#include "udp2lcm.h"
2
3void udpRecvHandler() {
4 int socketfd = -1;
5 char buffer[MAX_BUFFER_SIZE];
6 struct sockaddr_in serverAddr;
7 socklen_t addrLen = sizeof(serverAddr);
8 int bytesReceived;
9
10 int retval;
11 struct pollfd fds;
12
13 printf("udpRecvHandler\n");
14 if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
15 fprintf(stderr, "Error: Failed to create socket\n");
16 return;
17 }
18 memset((char *)&serverAddr, 0, sizeof(serverAddr));
19 memset(buffer, 0, sizeof(buffer));
20 serverAddr.sin_family = AF_INET;
21 serverAddr.sin_port = htons(PORT);
22 serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
23
24 if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
25 -1) {
26 fprintf(stderr, "Error: Bind failed\n");
27 close(socketfd);
28 return;
29 }
30
31 bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
32 (struct sockaddr *)&clientAddr, &addrLen);
33 if (bytesReceived == -1) {
34 fprintf(stderr, "Error: Failed to receive data\n");
35 close(socketfd);
36 return;
37 }
38 printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
39 buffer);
40 strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
41
42 /*
43 * 接收到来自手机端的第一条消息之后：
44 * - 启动udpSend线程，用于向手机端发送心跳信息
45 * - 通过ROBOT_COMTROL信道向算法模块发送建图开始消息
46 * - 7号命令初始坐标、初始角度、雷达扫描最大范围、机器人半径、机器人高
47 * - 30号命令开始建图，参数为单个像素点大小
48 */
49 if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) {
50 fprintf(stderr, "Error creating udpSend thread\n");
51 close(socketfd);
52 exit(-1);
53 }
54
55 robot_control_t robotCtrlData;
56 robotCtrlInit(&robotCtrlData, 0, 7, 0, 7, 1, 0, 0);
57 robotCtrlData.iparams[0] = 1;
58 robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
59 freeRobotCtrl(&robotCtrlData);
60 // 随即下达30号命令，开始建图
61 robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
62 robotCtrlData.dparams[0] = 0.05;
63 robotCtrlData.niparams = 1;
64 robotCtrlData.iparams[0] = 1;
65 robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
66 freeRobotCtrl(&robotCtrlData);
67
68 /*
69 * 开始接收来自手机端的命令
70 * 限定时间为3s，超过3s未收到消息则停止轮子
71 * 但不退出循环，防止是网络抖动
72 */
73 const path_ctrl_t path = {0, 0}; // 就是停止命令，不能改
74 fds.fd = socketfd;
75 fds.events = POLLIN;
76 while (true) {
77 retval = poll(&fds, 1, 3000);
78 if (retval == -1) {
79 fprintf(stderr, "Error: select failed\n");
80 break;
81 } else if (retval == 0) {
82 // 超出3s，报错并停止前进
83 fprintf(stderr, "No data within three seconds.\n");
84 path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
85 } else {
86 bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
87 (struct sockaddr *)&clientAddr, &addrLen);
88 if (bytesReceived == -1) {
89 fprintf(stderr, "Error: Failed to receive data\n");
90 continue;
91 }
92 printf("Massage from client %s: ", clientIP);
93 for (int i = 0; i < bytesReceived; i++) {
94 printf("%02x ", buffer[i]);
95 }
96 printf("\n");
97 parseCmd(buffer, bytesReceived);
98 }
99 }
100}
101
102void udpSendHandler() {
103 // 创建UDP套接字
104 int sockfd = -1;
105 struct sockaddr_in serverAddr;
106 if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
107 fprintf(stderr, "Error: Failed to create socket\n");
108 return;
109 }
110
111 memset((char *)&serverAddr, 0, sizeof(serverAddr));
112 serverAddr.sin_family = AF_INET; // 设置地址族为IPv4
113 serverAddr.sin_port = htons(PORT); // 设置端口号
114 if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
115 fprintf(stderr, "Error: Failed to convert IP address\n");
116 close(sockfd);
117 return;
118 }
119
120 while (true) {
121 pthread_mutex_lock(&heartBeatMutex);
122 sendto(sockfd, (const char *)heartBeat, 9, 0,
123 (struct sockaddr *)&serverAddr, sizeof(serverAddr));
124 pthread_mutex_unlock(&heartBeatMutex);
125 sleep(1);
126 }
127} \ No newline at end of file
diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
new file mode 100644
index 0000000..fdf32c8
--- /dev/null
+++ b/udp2lcm/udp2lcm.c
@@ -0,0 +1,118 @@
1// Encoded in UTF-8
2
3/*
4 * struct path_ctrl_t
5 * {
6 * int8_t cmd;
7 * int8_t speed;
8 * }
9 * 这是与轮控模块通信的数据结构，speed为0-100表示速度
10 * cmd为0停，1前，2左，3右
11 */
12
13#include "udp2lcm.h"
14
15pthread_t udpRecv, udpSend;
16int8_t heartBeat[9] = {0};
17pthread_mutex_t heartBeatMutex;
18struct sockaddr_in clientAddr;
19char clientIP[20] = "";
20lcm_t *lcm;
21
22int main() {
23 if (!LCMInit()) {
24 fprintf(stderr, "Error: Failed to initialize LCM\n");
25 return -1;
26 }
27 // 初始化线程锁
28 pthread_mutex_init(&heartBeatMutex, NULL);
29 // 开启udp接收线程
30 pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
31 pid_t httpServerPid = -1;
32 if ((httpServerPid = fork()) == 0) {
33 // 拉起服务器进程，手机端需要申请的文件/mnt/cf/mapfile/defaultMap.txt
34 // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
35 chdir("/mnt/cf/mapfile/");
36 /*
37 * execlp使用系统调用exec()执行一个程序
38 * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL
39 * 其他的是传递过去的参数，按一般约定，第一个参数是程序名，不被使用
40 * 因而这里出现了两个python
41 */
42 execlp("python", "python", "-m", "http.server", NULL);
43 }
44 while (true) {
45 pause();
46 }
47 return 0;
48}
49
50bool LCMInit() {
51 if ((lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1")) == NULL) {
52 fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
53 return false;
54 }
55 return true;
56}
57
58void parseCmd(const char *buffer, int bytesReceived) {
59 if (buffer == NULL) {
60 fprintf(stderr, "Error: Invalid message\n");
61 return;
62 }
63
64 path_ctrl_t path;
65 robot_control_t robotCtrlData;
66 pose_t curPos;
67 if (buffer[0] == 0) {
68 // 手机端用来与udp2lcm服务器建立连接的初始化消息
69 // 也是手机端的心跳
70 ;
71 } else if (buffer[0] == 1) {
72 // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
73 path.cmd = buffer[1];
74 path.speed = buffer[2];
75 path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
76 } else if (buffer[0] == 2) {
77 /*
78 * 结束建图，主要有以下几个任务：
79 * - 向轮控模块发送停止命令
80 * - 通知算法模块结束建图
81 * - 拉起http服务器进程（改由main拉起，省的每次拉起浪费资源）
82 * - 向手机端通知建图完成
83 * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
84 */
85 path.cmd = 0;
86 path.speed = 0;
87 path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
88
89 robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
90 robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
91
92 pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
93 } else if (buffer[0] == 3) {
94 // 手机发来的终点坐标，转发给算法模块
95 }
96}
97
98void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
99 const pose_t *msg, void *userdata) {
100 int16_t x, y, sita;
101 x = (int16_t)msg->pos[0];
102 y = (int16_t)msg->pos[1];
103 sita = (int16_t)msg->pos[2];
104 while (sita > 180)
105 sita -= 360;
106 while (sita < -180)
107 sita += 360;
108 pthread_mutex_lock(&heartBeatMutex);
109 heartBeat[0] = 0x03;
110 heartBeat[1] = heartBeat[2] = 0;
111 heartBeat[3] = x >> 8;
112 heartBeat[4] = x;
113 heartBeat[5] = y >> 8;
114 heartBeat[6] = y;
115 heartBeat[7] = sita >> 8;
116 heartBeat[8] = sita;
117 pthread_mutex_unlock(&heartBeatMutex);
118} \ No newline at end of file
diff --git a/udp2lcm/udp2lcm.h b/udp2lcm/udp2lcm.h
new file mode 100644
index 0000000..3bab6ac
--- /dev/null
+++ b/udp2lcm/udp2lcm.h
@@ -0,0 +1,45 @@
1#ifndef UDP2LCM_H
2#define UDP2LCM_H
3
4#include "path_ctrl_t.h"
5#include "robot_control_t.h"
6#include "pose_t.h"
7#include <arpa/inet.h>
8#include <errno.h>
9#include <fcntl.h>
10#include <netinet/in.h>
11#include <poll.h>
12#include <pthread.h>
13#include <stdarg.h>
14#include <stdbool.h>
15#include <stdio.h>
16#include <stdlib.h>
17#include <string.h>
18#include <sys/socket.h>
19#include <termios.h>
20#include <unistd.h>
21
22#define MAX_BUFFER_SIZE 1024
23#define PORT 5001
24
25extern pthread_t udpRecv, udpSend;
26extern int8_t heartBeat[9]; // 用于存储需要发送的心跳信息
27extern pthread_mutex_t heartBeatMutex; // 需要给heartBeat加线程锁
28// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改
29// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致
30extern struct sockaddr_in clientAddr;
31extern char clientIP[20];
32extern lcm_t *lcm;
33
34bool LCMInit();
35void udpSendHandler();
36void udpRecvHandler();
37void parseCmd(const char *buffer, int bytesReceived);
38void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
39 int8_t commandid, int8_t robotid, int8_t ndparams,
40 int8_t niparams, int8_t nsparams, int64_t nbparams);
41void freeRobotCtrl(robot_control_t *robotCtrlData);
42void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
43 const pose_t *msg, void *userdata);
44
45#endif \ No newline at end of file
generated by cgit v1.2.3-70-g09d2 (git 2.46.0) at 2025-04-05 00:47:01 +0800