From 1fb859960b3e9c17a78754c6321da85f6b7b01b6 Mon Sep 17 00:00:00 2001 From: We-unite <3205135446@qq.com> Date: Thu, 13 Jun 2024 18:19:08 +0800 Subject: add ROBOT_CONTROL, divide udp2lcm and serial --- CMakeLists.txt | 25 +-- lcmtype/eventlog.h | 150 ++++++++++++++ lcmtype/grid_map_t.cpp | 417 ++++++++++++++++++++++++++++++++++++++ lcmtype/grid_map_t.h | 157 +++++++++++++++ lcmtype/laser_t.cpp | 364 +++++++++++++++++++++++++++++++++ lcmtype/laser_t.h | 67 +++++++ lcmtype/lcm.h | 316 +++++++++++++++++++++++++++++ lcmtype/lcm_coretypes.h | 460 ++++++++++++++++++++++++++++++++++++++++++ lcmtype/lcmtype.h | 14 ++ lcmtype/lcmtype.mk | 22 ++ lcmtype/path_ctrl.lcm | 5 + lcmtype/path_ctrl_t.c | 241 ++++++++++++++++++++++ lcmtype/path_ctrl_t.h | 142 +++++++++++++ lcmtype/path_t.cpp | 297 +++++++++++++++++++++++++++ lcmtype/path_t.h | 60 ++++++ lcmtype/pose_t.cpp | 325 ++++++++++++++++++++++++++++++ lcmtype/pose_t.h | 63 ++++++ lcmtype/robot_control_t.cpp | 478 ++++++++++++++++++++++++++++++++++++++++++++ lcmtype/robot_control_t.h | 76 +++++++ lib/aarch64/liblcm.so | Bin 0 -> 95736 bytes lib/liblcm.so | Bin 144320 -> 0 bytes lib/x86/liblcm.so | Bin 0 -> 144320 bytes path/path_ctrl.lcm | 5 - path/path_ctrl_t.c | 241 ---------------------- path/path_ctrl_t.h | 143 ------------- serial.c | 130 ------------ serial/CMakeLists.txt | 7 + serial/serial.c | 130 ++++++++++++ udp2lcm.c | 198 ------------------ udp2lcm/CMakeLists.txt | 10 + udp2lcm/robotSet.c | 60 ++++++ udp2lcm/udp.c | 127 ++++++++++++ udp2lcm/udp2lcm.c | 118 +++++++++++ udp2lcm/udp2lcm.h | 45 +++++ 34 files changed, 4164 insertions(+), 729 deletions(-) create mode 100644 lcmtype/eventlog.h create mode 100644 lcmtype/grid_map_t.cpp create mode 100644 lcmtype/grid_map_t.h create mode 100644 lcmtype/laser_t.cpp create mode 100644 lcmtype/laser_t.h create mode 100644 lcmtype/lcm.h create mode 100644 lcmtype/lcm_coretypes.h create mode 100644 lcmtype/lcmtype.h create mode 100644 lcmtype/lcmtype.mk create mode 100644 lcmtype/path_ctrl.lcm create mode 100644 lcmtype/path_ctrl_t.c create mode 100644 lcmtype/path_ctrl_t.h create mode 100644 lcmtype/path_t.cpp create mode 100644 lcmtype/path_t.h create mode 100644 lcmtype/pose_t.cpp create mode 100644 lcmtype/pose_t.h create mode 100644 lcmtype/robot_control_t.cpp create mode 100644 lcmtype/robot_control_t.h create mode 100644 lib/aarch64/liblcm.so delete mode 100755 lib/liblcm.so create mode 100755 lib/x86/liblcm.so delete mode 100644 path/path_ctrl.lcm delete mode 100644 path/path_ctrl_t.c delete mode 100644 path/path_ctrl_t.h delete mode 100644 serial.c create mode 100644 serial/CMakeLists.txt create mode 100644 serial/serial.c delete mode 100644 udp2lcm.c create mode 100644 udp2lcm/CMakeLists.txt create mode 100644 udp2lcm/robotSet.c create mode 100644 udp2lcm/udp.c create mode 100644 udp2lcm/udp2lcm.c create mode 100644 udp2lcm/udp2lcm.h diff --git a/CMakeLists.txt b/CMakeLists.txt index da5fc52..c487036 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,17 +1,18 @@ cmake_minimum_required(VERSION 3.10) project(WheelCtrl) -# Set the output directory for the build executables. -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) +# 设置编译参数 add_compile_options(-g -w) +# 添加头文件路径 +include_directories( + ${CMAKE_SOURCE_DIR}/lcmtype +) +# 添加库文件路径 +link_directories( + ${CMAKE_SOURCE_DIR}/lib +) +# 设置输出路径 +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) -# Find the required libraries -find_library(LCM_LIBRARY lcm lib) - -# Add the executable serial -add_executable(serial serial.c path/path_ctrl_t.c) -target_link_libraries(serial ${LCM_LIBRARY}) - -# Add the executable udp2lcm -add_executable(udp2lcm udp2lcm.c path/path_ctrl_t.c) -target_link_libraries(udp2lcm ${LCM_LIBRARY}) \ No newline at end of file +add_subdirectory(serial) +add_subdirectory(udp2lcm) \ 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 @@ +#ifndef _LCM_EVENTLOG_H_ +#define _LCM_EVENTLOG_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef LCM_API_FUNCTION +#ifdef WIN32 +#define LCM_API_FUNCTION __declspec(dllexport) +#else +#define LCM_API_FUNCTION +#endif // WIN32 +#endif // LCM_API_FUNCTION + +/** + * @defgroup LcmC_lcm_eventlog_t lcm_eventlog_t + * @ingroup LcmC + * @brief Read and write %LCM log files + * + * #include + * + * Linking: `pkg-config --libs lcm` + * + * @{ + */ + +typedef struct _lcm_eventlog_t lcm_eventlog_t; +struct _lcm_eventlog_t { + /** + * The underlying file handle. Use this at your own risk. Example use + * cases include implementing your own seek routine for read-mode logs, or + * rewinding the file pointer to the beginning of the log file. + */ + FILE *f; + /** + * Do not use. + */ + int64_t eventcount; +}; + +/** + * Represents a single event (message) in a log file. + */ +typedef struct _lcm_eventlog_event_t lcm_eventlog_event_t; +struct _lcm_eventlog_event_t { + /** + * A monotonically increasing number assigned to the message to identify it + * in the log file. + */ + int64_t eventnum; + /** + * Time that the message was received, in microseconds since the UNIX + * epoch + */ + int64_t timestamp; + /** + * Length of @c channel, in bytes + */ + int32_t channellen; + /** + * Length of @c data, in bytes + */ + int32_t datalen; + + /** + * Channel that the message was received on + */ + char *channel; + /** + * Raw byte buffer containing the message payload. + */ + void *data; +}; + +/** + * Open a log file for reading or writing. + * + * @param path Log file to open + * @param mode "r" (read mode) or "w" (write mode) + * + * @return a newly allocated lcm_eventlog_t, or NULL on failure. + */ +LCM_API_FUNCTION +lcm_eventlog_t *lcm_eventlog_create(const char *path, const char *mode); + +/** + * Read the next event in the log file. Valid in read mode only. Free the + * returned structure with lcm_eventlog_free_event() after use. + * + * @param eventlog The log file object + * + * @return the next event in the log file, or NULL when the end of the file has + * been reached. + */ +LCM_API_FUNCTION +lcm_eventlog_event_t *lcm_eventlog_read_next_event(lcm_eventlog_t *eventlog); + +/** + * Free a structure returned by lcm_eventlog_read_next_event(). + * + * @param event A structure returned by lcm_eventlog_read_next_event() + */ +LCM_API_FUNCTION +void lcm_eventlog_free_event(lcm_eventlog_event_t *event); + +/** + * Seek (approximately) to a particular timestamp. + * + * @param eventlog The log file object + * @param ts Timestamp of the target event in the log file. + * + * @return 0 on success, -1 on failure + */ +LCM_API_FUNCTION +int lcm_eventlog_seek_to_timestamp(lcm_eventlog_t *eventlog, int64_t ts); + +/** + * Write an event into a log file. Valid in write mode only. + * + * @param eventlog The log file object + * @param event The event to write to the file. On return, the eventnum field + * will be filled in for you. + * + * @return 0 on success, -1 on failure. + */ +LCM_API_FUNCTION +int lcm_eventlog_write_event(lcm_eventlog_t *eventlog, + lcm_eventlog_event_t *event); + +/** + * Close a log file and release allocated resources. + * + * @param eventlog The log file object + */ +LCM_API_FUNCTION +void lcm_eventlog_destroy(lcm_eventlog_t *eventlog); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#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 @@ +// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY +// BY HAND!! +// +// Generated by lcm-gen + +#include "grid_map_t.h" +#include + +static int __grid_map_t_hash_computed; +static uint64_t __grid_map_t_hash; + +uint64_t __grid_map_t_hash_recursive(const __lcm_hash_ptr *p) { + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __grid_map_t_get_hash) + return 0; + + __lcm_hash_ptr cp; + cp.parent = p; + cp.v = (void *)__grid_map_t_get_hash; + (void)cp; + + uint64_t hash = + (uint64_t)0x3b95cdd95c1cd816LL + __int64_t_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + __double_hash_recursive(&cp) + + __double_hash_recursive(&cp) + __double_hash_recursive(&cp) + + __int32_t_hash_recursive(&cp) + __int32_t_hash_recursive(&cp) + + __int32_t_hash_recursive(&cp) + __string_hash_recursive(&cp) + + __byte_hash_recursive(&cp); + + return (hash << 1) + ((hash >> 63) & 1); +} + +int64_t __grid_map_t_get_hash(void) { + if (!__grid_map_t_hash_computed) { + __grid_map_t_hash = (int64_t)__grid_map_t_hash_recursive(NULL); + __grid_map_t_hash_computed = 1; + } + + return __grid_map_t_hash; +} + +int __grid_map_t_encode_array(void *buf, int offset, int maxlen, + const grid_map_t *p, int elements) { + int pos = 0, element; + int thislen; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].encoding), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].x0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].y0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].meters_per_pixel), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].width), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].height), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].datalen), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __string_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].src), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __byte_encode_array(buf, offset + pos, maxlen - pos, + p[element].dst, p[element].datalen); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int grid_map_t_encode(void *buf, int offset, int maxlen, const grid_map_t *p) { + int pos = 0, thislen; + int64_t hash = __grid_map_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __grid_map_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int __grid_map_t_encoded_array_size(const grid_map_t *p, int elements) { + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int64_t_encoded_array_size(&(p[element].utime), 1); + + size += __int8_t_encoded_array_size(&(p[element].encoding), 1); + + size += __double_encoded_array_size(&(p[element].x0), 1); + + size += __double_encoded_array_size(&(p[element].y0), 1); + + size += __double_encoded_array_size(&(p[element].meters_per_pixel), 1); + + size += __int32_t_encoded_array_size(&(p[element].width), 1); + + size += __int32_t_encoded_array_size(&(p[element].height), 1); + + size += __int32_t_encoded_array_size(&(p[element].datalen), 1); + + size += __string_encoded_array_size(&(p[element].src), 1); + + size += __byte_encoded_array_size(p[element].dst, p[element].datalen); + } + return size; +} + +int grid_map_t_encoded_size(const grid_map_t *p) { + return 8 + __grid_map_t_encoded_array_size(p, 1); +} + +int __grid_map_t_decode_array(const void *buf, int offset, int maxlen, + grid_map_t *p, int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].encoding), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].x0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].y0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].meters_per_pixel), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].width), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].height), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].datalen), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __string_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].src), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].dst = + (uint8_t *)lcm_malloc(sizeof(uint8_t) * p[element].datalen); + thislen = __byte_decode_array(buf, offset + pos, maxlen - pos, + p[element].dst, p[element].datalen); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int __grid_map_t_decode_array_cleanup(grid_map_t *p, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_decode_array_cleanup(&(p[element].utime), 1); + + __int8_t_decode_array_cleanup(&(p[element].encoding), 1); + + __double_decode_array_cleanup(&(p[element].x0), 1); + + __double_decode_array_cleanup(&(p[element].y0), 1); + + __double_decode_array_cleanup(&(p[element].meters_per_pixel), 1); + + __int32_t_decode_array_cleanup(&(p[element].width), 1); + + __int32_t_decode_array_cleanup(&(p[element].height), 1); + + __int32_t_decode_array_cleanup(&(p[element].datalen), 1); + + __string_decode_array_cleanup(&(p[element].src), 1); + + __byte_decode_array_cleanup(p[element].dst, p[element].datalen); + if (p[element].dst) + free(p[element].dst); + } + return 0; +} + +int grid_map_t_decode(const void *buf, int offset, int maxlen, grid_map_t *p) { + int pos = 0, thislen; + int64_t hash = __grid_map_t_get_hash(); + + int64_t this_hash; + thislen = + __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + if (this_hash != hash) + return -1; + + thislen = __grid_map_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int grid_map_t_decode_cleanup(grid_map_t *p) { + return __grid_map_t_decode_array_cleanup(p, 1); +} + +int __grid_map_t_clone_array(const grid_map_t *p, grid_map_t *q, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1); + + __int8_t_clone_array(&(p[element].encoding), &(q[element].encoding), 1); + + __double_clone_array(&(p[element].x0), &(q[element].x0), 1); + + __double_clone_array(&(p[element].y0), &(q[element].y0), 1); + + __double_clone_array(&(p[element].meters_per_pixel), + &(q[element].meters_per_pixel), 1); + + __int32_t_clone_array(&(p[element].width), &(q[element].width), 1); + + __int32_t_clone_array(&(p[element].height), &(q[element].height), 1); + + __int32_t_clone_array(&(p[element].datalen), &(q[element].datalen), 1); + + __string_clone_array(&(p[element].src), &(q[element].src), 1); + + q[element].dst = + (uint8_t *)lcm_malloc(sizeof(uint8_t) * q[element].datalen); + __byte_clone_array(p[element].dst, q[element].dst, p[element].datalen); + } + return 0; +} + +grid_map_t *grid_map_t_copy(const grid_map_t *p) { + grid_map_t *q = (grid_map_t *)malloc(sizeof(grid_map_t)); + __grid_map_t_clone_array(p, q, 1); + return q; +} + +void grid_map_t_destroy(grid_map_t *p) { + __grid_map_t_decode_array_cleanup(p, 1); + free(p); +} + +int grid_map_t_publish(lcm_t *lc, const char *channel, const grid_map_t *p) { + int max_data_size = grid_map_t_encoded_size(p); + uint8_t *buf = (uint8_t *)malloc(max_data_size); + if (!buf) + return -1; + int data_size = grid_map_t_encode(buf, 0, max_data_size, p); + if (data_size < 0) { + free(buf); + return data_size; + } + int status = lcm_publish(lc, channel, buf, data_size); + free(buf); + return status; +} + +struct _grid_map_t_subscription_t { + grid_map_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static void grid_map_t_handler_stub(const lcm_recv_buf_t *rbuf, + const char *channel, void *userdata) { + int status; + grid_map_t p; + memset(&p, 0, sizeof(grid_map_t)); + status = grid_map_t_decode(rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf(stderr, "error %d decoding grid_map_t!!!\n", status); + return; + } + + grid_map_t_subscription_t *h = (grid_map_t_subscription_t *)userdata; + h->user_handler(rbuf, channel, &p, h->userdata); + + grid_map_t_decode_cleanup(&p); +} + +grid_map_t_subscription_t *grid_map_t_subscribe(lcm_t *lcm, const char *channel, + grid_map_t_handler_t f, + void *userdata) { + grid_map_t_subscription_t *n = + (grid_map_t_subscription_t *)malloc(sizeof(grid_map_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe(lcm, channel, grid_map_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf(stderr, "couldn't reg grid_map_t LCM handler!\n"); + free(n); + return NULL; + } + return n; +} + +int grid_map_t_subscription_set_queue_capacity(grid_map_t_subscription_t *subs, + int num_messages) { + return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages); +} + +int grid_map_t_unsubscribe(lcm_t *lcm, grid_map_t_subscription_t *hid) { + int status = lcm_unsubscribe(lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, "couldn't unsubscribe grid_map_t_handler %p!\n", hid); + return -1; + } + free(hid); + return 0; +} 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 @@ +// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY +// BY HAND!! +// +// Generated by lcm-gen + +// #include +// #include +#include "lcmtype.h" +#include +#include + + +#ifndef _grid_map_t_h +#define _grid_map_t_h + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _grid_map_t grid_map_t; +struct _grid_map_t { + int64_t utime; + int8_t encoding; + double x0; + double y0; + double meters_per_pixel; + int32_t width; + int32_t height; + int32_t datalen; + char *src; + uint8_t *dst; +}; + +/** + * Create a deep copy of a grid_map_t. + * When no longer needed, destroy it with grid_map_t_destroy() + */ +grid_map_t *grid_map_t_copy(const grid_map_t *to_copy); + +/** + * Destroy an instance of grid_map_t created by grid_map_t_copy() + */ +void grid_map_t_destroy(grid_map_t *to_destroy); + +/** + * Identifies a single subscription. This is an opaque data type. + */ +typedef struct _grid_map_t_subscription_t grid_map_t_subscription_t; + +/** + * Prototype for a callback function invoked when a message of type + * grid_map_t is received. + */ +typedef void (*grid_map_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, const grid_map_t *msg, + void *userdata); + +/** + * Publish a message of type grid_map_t using LCM. + * + * @param lcm The LCM instance to publish with. + * @param channel The channel to publish on. + * @param msg The message to publish. + * @return 0 on success, <0 on error. Success means LCM has transferred + * responsibility of the message data to the OS. + */ +int grid_map_t_publish(lcm_t *lcm, const char *channel, const grid_map_t *msg); + +/** + * Subscribe to messages of type grid_map_t using LCM. + * + * @param lcm The LCM instance to subscribe with. + * @param channel The channel to subscribe to. + * @param handler The callback function invoked by LCM when a message is + * received. This function is invoked by LCM during calls to lcm_handle() and + * lcm_handle_timeout(). + * @param userdata An opaque pointer passed to @p handler when it is invoked. + * @return 0 on success, <0 if an error occured + */ +grid_map_t_subscription_t *grid_map_t_subscribe(lcm_t *lcm, const char *channel, + grid_map_t_handler_t handler, + void *userdata); + +/** + * Removes and destroys a subscription created by grid_map_t_subscribe() + */ +int grid_map_t_unsubscribe(lcm_t *lcm, grid_map_t_subscription_t *hid); + +/** + * Sets the queue capacity for a subscription. + * Some LCM providers (e.g., the default multicast provider) are implemented + * using a background receive thread that constantly revceives messages from + * the network. As these messages are received, they are buffered on + * per-subscription queues until dispatched by lcm_handle(). This function + * how many messages are queued before dropping messages. + * + * @param subs the subscription to modify. + * @param num_messages The maximum number of messages to queue + * on the subscription. + * @return 0 on success, <0 if an error occured + */ +int grid_map_t_subscription_set_queue_capacity(grid_map_t_subscription_t *subs, + int num_messages); + +/** + * Encode a message of type grid_map_t into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at this byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally + * be equal to grid_map_t_encoded_size(). + * @param msg The message to encode. + * @return The number of bytes encoded, or <0 if an error occured. + */ +int grid_map_t_encode(void *buf, int offset, int maxlen, const grid_map_t *p); + +/** + * Decode a message of type grid_map_t from binary form. + * When decoding messages containing strings or variable-length arrays, this + * function may allocate memory. When finished with the decoded message, + * release allocated resources with grid_map_t_decode_cleanup(). + * + * @param buf The buffer containing the encoded message + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @param msg Output parameter where the decoded message is stored + * @return The number of bytes decoded, or <0 if an error occured. + */ +int grid_map_t_decode(const void *buf, int offset, int maxlen, grid_map_t *msg); + +/** + * Release resources allocated by grid_map_t_decode() + * @return 0 + */ +int grid_map_t_decode_cleanup(grid_map_t *p); + +/** + * Check how many bytes are required to encode a message of type grid_map_t + */ +int grid_map_t_encoded_size(const grid_map_t *p); + +// LCM support functions. Users should not call these +int64_t __grid_map_t_get_hash(void); +uint64_t __grid_map_t_hash_recursive(const __lcm_hash_ptr *p); +int __grid_map_t_encode_array(void *buf, int offset, int maxlen, + const grid_map_t *p, int elements); +int __grid_map_t_decode_array(const void *buf, int offset, int maxlen, + grid_map_t *p, int elements); +int __grid_map_t_decode_array_cleanup(grid_map_t *p, int elements); +int __grid_map_t_encoded_array_size(const grid_map_t *p, int elements); +int __grid_map_t_clone_array(const grid_map_t *p, grid_map_t *q, int elements); + +#ifdef __cplusplus +} +#endif + +#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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "laser_t.h" +#include + +static int __laser_t_hash_computed; +static int64_t __laser_t_hash; + +int64_t __laser_t_hash_recursive(const __lcm_hash_ptr *p) { + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __laser_t_get_hash) + return 0; + + const __lcm_hash_ptr cp = {p, (void *)__laser_t_get_hash}; + (void)cp; + + int64_t hash = 0xf1e8ba118c05af46LL + __int64_t_hash_recursive(&cp) + + __int32_t_hash_recursive(&cp) + __float_hash_recursive(&cp) + + __int32_t_hash_recursive(&cp) + __float_hash_recursive(&cp) + + __float_hash_recursive(&cp) + __float_hash_recursive(&cp); + + return (hash << 1) + ((hash >> 63) & 1); +} + +int64_t __laser_t_get_hash(void) { + if (!__laser_t_hash_computed) { + __laser_t_hash = __laser_t_hash_recursive(NULL); + __laser_t_hash_computed = 1; + } + + return __laser_t_hash; +} + +int __laser_t_encode_array(void *buf, int offset, int maxlen, const laser_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].nranges), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_encode_array(buf, offset + pos, maxlen - pos, + p[element].ranges, p[element].nranges); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].nintensities), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_encode_array(buf, offset + pos, maxlen - pos, + p[element].intensities, + p[element].nintensities); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].rad0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].radstep), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int laser_t_encode(void *buf, int offset, int maxlen, const laser_t *p) { + int pos = 0, thislen; + int64_t hash = __laser_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __laser_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int __laser_t_encoded_array_size(const laser_t *p, int elements) { + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int64_t_encoded_array_size(&(p[element].utime), 1); + + size += __int32_t_encoded_array_size(&(p[element].nranges), 1); + + size += + __float_encoded_array_size(p[element].ranges, p[element].nranges); + + size += __int32_t_encoded_array_size(&(p[element].nintensities), 1); + + size += __float_encoded_array_size(p[element].intensities, + p[element].nintensities); + + size += __float_encoded_array_size(&(p[element].rad0), 1); + + size += __float_encoded_array_size(&(p[element].radstep), 1); + } + return size; +} + +int laser_t_encoded_size(const laser_t *p) { + return 8 + __laser_t_encoded_array_size(p, 1); +} + +int __laser_t_decode_array(const void *buf, int offset, int maxlen, laser_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].nranges), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].ranges = + (float *)lcm_malloc(sizeof(float) * p[element].nranges); + thislen = __float_decode_array(buf, offset + pos, maxlen - pos, + p[element].ranges, p[element].nranges); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int32_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].nintensities), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].intensities = + (float *)lcm_malloc(sizeof(float) * p[element].nintensities); + thislen = __float_decode_array(buf, offset + pos, maxlen - pos, + p[element].intensities, + p[element].nintensities); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].rad0), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __float_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].radstep), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int __laser_t_decode_array_cleanup(laser_t *p, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_decode_array_cleanup(&(p[element].utime), 1); + + __int32_t_decode_array_cleanup(&(p[element].nranges), 1); + + __float_decode_array_cleanup(p[element].ranges, p[element].nranges); + if (p[element].ranges) + free(p[element].ranges); + + __int32_t_decode_array_cleanup(&(p[element].nintensities), 1); + + __float_decode_array_cleanup(p[element].intensities, + p[element].nintensities); + if (p[element].intensities) + free(p[element].intensities); + + __float_decode_array_cleanup(&(p[element].rad0), 1); + + __float_decode_array_cleanup(&(p[element].radstep), 1); + } + return 0; +} + +int laser_t_decode(const void *buf, int offset, int maxlen, laser_t *p) { + int pos = 0, thislen; + int64_t hash = __laser_t_get_hash(); + + int64_t this_hash; + thislen = + __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + if (this_hash != hash) + return -1; + + thislen = __laser_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int laser_t_decode_cleanup(laser_t *p) { + return __laser_t_decode_array_cleanup(p, 1); +} + +int __laser_t_clone_array(const laser_t *p, laser_t *q, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1); + + __int32_t_clone_array(&(p[element].nranges), &(q[element].nranges), 1); + + q[element].ranges = + (float *)lcm_malloc(sizeof(float) * q[element].nranges); + __float_clone_array(p[element].ranges, q[element].ranges, + p[element].nranges); + + __int32_t_clone_array(&(p[element].nintensities), + &(q[element].nintensities), 1); + + q[element].intensities = + (float *)lcm_malloc(sizeof(float) * q[element].nintensities); + __float_clone_array(p[element].intensities, q[element].intensities, + p[element].nintensities); + + __float_clone_array(&(p[element].rad0), &(q[element].rad0), 1); + + __float_clone_array(&(p[element].radstep), &(q[element].radstep), 1); + } + return 0; +} + +laser_t *laser_t_copy(const laser_t *p) { + laser_t *q = (laser_t *)malloc(sizeof(laser_t)); + __laser_t_clone_array(p, q, 1); + return q; +} + +void laser_t_destroy(laser_t *p) { + __laser_t_decode_array_cleanup(p, 1); + free(p); +} + +int laser_t_publish(lcm_t *lc, const char *channel, const laser_t *p) { + int max_data_size = laser_t_encoded_size(p); + uint8_t *buf = (uint8_t *)malloc(max_data_size); + if (!buf) + return -1; + int data_size = laser_t_encode(buf, 0, max_data_size, p); + if (data_size < 0) { + free(buf); + return data_size; + } + int status = lcm_publish(lc, channel, buf, data_size); + free(buf); + return status; +} + +struct _laser_t_subscription_t { + laser_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static void laser_t_handler_stub(const lcm_recv_buf_t *rbuf, + const char *channel, void *userdata) { + int status; + laser_t p; + memset(&p, 0, sizeof(laser_t)); + status = laser_t_decode(rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf(stderr, "error %d decoding laser_t!!!\n", status); + return; + } + + laser_t_subscription_t *h = (laser_t_subscription_t *)userdata; + h->user_handler(rbuf, channel, &p, h->userdata); + + laser_t_decode_cleanup(&p); +} + +laser_t_subscription_t *laser_t_subscribe(lcm_t *lcm, const char *channel, + laser_t_handler_t f, void *userdata) { + laser_t_subscription_t *n = + (laser_t_subscription_t *)malloc(sizeof(laser_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe(lcm, channel, laser_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf(stderr, "couldn't reg laser_t LCM handler!\n"); + free(n); + return NULL; + } + return n; +} + +int laser_t_subscription_set_queue_capacity(laser_t_subscription_t *subs, + int num_messages) { + return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages); +} + +int laser_t_unsubscribe(lcm_t *lcm, laser_t_subscription_t *hid) { + int status = lcm_unsubscribe(lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, "couldn't unsubscribe laser_t_handler %p!\n", hid); + return -1; + } + free(hid); + return 0; +} 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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "lcm.h" +#include "lcmtype.h" +#include +#include + +#ifndef _laser_t_h +#define _laser_t_h + +// 若为C++代码，需加上extern "C"，表明以下按照C的方式进行编译 +// 否则会出现链接错误 +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _laser_t laser_t; +struct _laser_t { + int64_t utime; + int32_t nranges; + float *ranges; + int32_t nintensities; + float *intensities; + float rad0; + float radstep; +}; + +laser_t *laser_t_copy(const laser_t *p); +void laser_t_destroy(laser_t *p); + +typedef struct _laser_t_subscription_t laser_t_subscription_t; +typedef void (*laser_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, const laser_t *msg, + void *user); + +int laser_t_publish(lcm_t *lcm, const char *channel, const laser_t *p); +laser_t_subscription_t *laser_t_subscribe(lcm_t *lcm, const char *channel, + laser_t_handler_t f, void *userdata); +int laser_t_unsubscribe(lcm_t *lcm, laser_t_subscription_t *hid); +int laser_t_subscription_set_queue_capacity(laser_t_subscription_t *subs, + int num_messages); + +int laser_t_encode(void *buf, int offset, int maxlen, const laser_t *p); +int laser_t_decode(const void *buf, int offset, int maxlen, laser_t *p); +int laser_t_decode_cleanup(laser_t *p); +int laser_t_encoded_size(const laser_t *p); + +// LCM support functions. Users should not call these +int64_t __laser_t_get_hash(void); +int64_t __laser_t_hash_recursive(const __lcm_hash_ptr *p); +int __laser_t_encode_array(void *buf, int offset, int maxlen, const laser_t *p, + int elements); +int __laser_t_decode_array(const void *buf, int offset, int maxlen, laser_t *p, + int elements); +int __laser_t_decode_array_cleanup(laser_t *p, int elements); +int __laser_t_encoded_array_size(const laser_t *p, int elements); +int __laser_t_clone_array(const laser_t *p, laser_t *q, int elements); + +#ifdef __cplusplus +} +#endif + +#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 @@ +#ifndef __lightweight_comunications_h__ +#define __lightweight_comunications_h__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "eventlog.h" + +#define LCM_MAX_MESSAGE_SIZE (1 << 28) + +#define LCM_MAX_CHANNEL_NAME_LENGTH 63 + +#ifdef WIN32 +#define LCM_API_FUNCTION __declspec(dllexport) +#else +#define LCM_API_FUNCTION +#endif + +/** + * @defgroup LcmC C API Reference + * + * THe %LCM C API provides classes and data structures for communicating with + * other %LCM clients, as well as reading and writing %LCM log files. + * + */ + +/** + * @defgroup LcmC_lcm_t lcm_t + * @ingroup LcmC + * @brief Publish and receive messages + * + * All %LCM functions are internally synchronized and thread-safe. + * + * #include + * + * Linking: `pkg-config --libs lcm` + * @{ + */ + +/** + * Opaque data structure containing the LCM context. + */ +typedef struct _lcm_t lcm_t; + +/** + * An opaque data structure that identifies an LCM subscription. + */ +typedef struct _lcm_subscription_t lcm_subscription_t; + +/** + * Received messages are passed to user programs using this data structure. + * Each instance represents one message. + */ +typedef struct _lcm_recv_buf_t lcm_recv_buf_t; +struct _lcm_recv_buf_t { + /** + * the data received (raw bytes) + */ + void *data; + /** + * the length of the data received (in bytes) + */ + uint32_t data_size; + /** + * timestamp (micrseconds since the epoch) at which the first data + * bytes of the message were received. + */ + int64_t recv_utime; + /** + * pointer to the lcm_t struct that owns this buffer + */ + lcm_t *lcm; +}; + +/** + * @brief Callback function prototype. + * + * Pass instances of this to lcm_subscribe() + * + * @param rbuf the message timestamp and payload + * @param channel the channel the message was received on + * @param user_data the user-specified parameter passed to lcm_subscribe() + */ +typedef void (*lcm_msg_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, void *user_data); + +/** + * @brief Constructor + * + * Allocates and initializes a lcm_t. %provider must be either + * NULL, or a string of the form + * + * "provider://network?option1=value1&option2=value2&...&optionN=valueN" + * + * @param provider Initializationg string specifying the LCM network provider. + * If this is NULL, and the environment variable "LCM_DEFAULT_URL" is defined, + * then the environment variable is used instead. If this is NULL and the + * environment variable is not defined, then default settings are used. + * + * The currently supported providers are: + * + * @verbatim + udpm:// + UDP Multicast provider + network can be of the form "multicast_address:port". Either the + multicast address or the port may be ommitted for the default. + + options: + recv_buf_size = N + size of the kernel UDP receive buffer to request. Defaults to + operating system defaults + + ttl = N + time to live of transmitted packets. Default 0 + + examples: + "udpm://239.255.76.67:7667" + Default initialization string + + "udpm://239.255.76.67:7667?ttl=1" + Sets the multicast TTL to 1 so that packets published will enter + the local network. + @endverbatim + * + * @verbatim + file:// + LCM Log file-based provider + network should be the path to the log file + + Events are read from or written to the log file. In read mode, events + are generated from the log file in real-time, or at the rate specified + by the speed option. In write mode, events published to the LCM instance + will be written to the log file in real-time. + + options: + speed = N + Scale factor controlling the playback speed of the log file. + Defaults to 1. If less than or equal to zero, then the events + in the log file are played back as fast as possible. Events are + never skipped in read mode, so actual playback speed may be slower + than requested, depending on the handlers. + + mode = r | w + Specifies the log file mode. Defaults to 'r' + + start_timestamp = USEC + Seeks to USEC microseconds in the logfile, where USEC is given in + microseconds since 00:00:00 UTC on 1 January 1970. If USEC is + before the first event, then playback begins at the start of the + log file. If it is after the last event, calls to lcm_handle will + return -1. + + examples: + "file:///home/albert/path/to/logfile" + Loads the file "/home/albert/path/to/logfile" as an LCM event + source. + + "file:///home/albert/path/to/logfile?speed=4" + Loads the file "/home/albert/path/to/logfile" as an LCM event + source. Events are played back at 4x speed. + + @endverbatim + * + * @return a newly allocated lcm_t instance, or NULL on failure. Free with + * lcm_destroy() when no longer needed. + */ +LCM_API_FUNCTION +lcm_t *lcm_create(const char *provider); + +/** + * @brief Destructor + */ +LCM_API_FUNCTION +void lcm_destroy(lcm_t *lcm); + +/** + * @brief Returns a file descriptor or socket that can be used with + * @c select(), @c poll(), or other event loops for asynchronous + * notification of incoming messages. + * + * Each LCM instance has a file descriptor that can be used to asynchronously + * receive notification when incoming messages have been received. This file + * descriptor can typically be incorporated into an application event loop + * (e.g., GTK+, QT, etc.) For an example using select(), see + * examples/c/listener-async.c + * + * @return a file descriptor suitable for use with select, poll, etc. + */ +LCM_API_FUNCTION +int lcm_get_fileno(lcm_t *lcm); + +/** + * @brief Subscribe a callback function to a channel, without automatic message + * decoding. + * + * In general, you probably don't want to use this function, as it does not + * automatically decode messages. Instead, use the message-specific subscribe + * function generated by @c lcm-gen. Use this function only when you want to + * work with the raw message itself. TODO link to example or more details. + * + * The callback function will be invoked during calls to lcm_handle() any time + * a message on the specified channel is received. Multiple callbacks can be + * subscribed for the same channel. + * + * @param lcm the LCM object + * @param channel the channel to listen on. This can also be a GLib regular + * expression, and is treated as a regex implicitly surrounded + * by '^' and '$' + * @param handler the callback function to be invoked when a message is + * received on the specified channel + * @param userdata this will be passed to the callback function + * + * @return a lcm_subscription_t to identify the new subscription, + * which can be passed to lcm_unsubscribe(). The lcm_t instance owns + * the subscription object. + */ +LCM_API_FUNCTION +lcm_subscription_t *lcm_subscribe(lcm_t *lcm, const char *channel, + lcm_msg_handler_t handler, void *userdata); + +/** + * @brief Unsubscribe a message handler. + * + * In general, you probably don't want to use this function. Instead, use the + * message-specific unsubscribe function generated by @c lcm-gen. Use this + * function only when you want to work with the raw message itself. TODO link + * to example or more details. + * + * The callback function for the subscription will no longer be + * invoked when messages on the corresponding channel are received. After this + * function returns, @c handler is no longer valid and should not be used + * anymore. + * + * @return 0 on success, or -1 if @c handler is not a valid subscription. + */ +LCM_API_FUNCTION +int lcm_unsubscribe(lcm_t *lcm, lcm_subscription_t *handler); + +/** + * @brief Publish a message, specified as a raw byte buffer. + * + * In general, you probably don't want to use this function, as it does not + * automatically encode messages. Instead, use the message-specific publish + * function generated by @c lcm-gen. Use this function only when you want to + * publish raw byte buffers. TODO link to example or more details. + * + * @param lcm The %LCM object + * @param channel The channel to publish on + * @param data The raw byte buffer + * @param datalen Size of the byte buffer + * + * @return 0 on success, -1 on failure. + */ +LCM_API_FUNCTION +int lcm_publish(lcm_t *lcm, const char *channel, const void *data, + unsigned int datalen); + +/** + * @brief Wait for and dispatch the next incoming message. + * + * Message handlers are invoked one at a time from the thread that calls this + * function, and in the order that they were subscribed. + * + * This function waits indefinitely. If you want timeout behavior, (e.g., wait + * 100ms for a message) then consider using lcm_get_fileno() together with + * select() or poll() + * + * Recursive calls to lcm_handle are not allowed -- do not call lcm_handle from + * within a message handler. All other functions are okay (e.g., it is okay to + * call lcm_publish from within a message handler). + * + * @param lcm the %LCM object + * + * @return 0 normally, or -1 when an error has occurred. + */ +LCM_API_FUNCTION +int lcm_handle(lcm_t *lcm); + +/** + * @brief Adjusts the maximum number of received messages that can be queued up + * for a subscription. + * + * In general, you probably don't want to use this function. Instead, use the + * message-specific set_queue_capacity function generated by @c lcm-gen. Use + * this function only when you want to work with untyped subscriptions. TODO + * link to example or more details. + * + * Setting this to a low number may reduce overall latency at the expense of + * dropping more messages. Conversely, setting this to a high number may drop + * fewer messages at the expense of increased latency. A value of 0 indicates + * no limit, and should be used carefully. + * + * @param handler the subscription object + * @param num_messages the maximum queue size, in messages. The default is 30. + * + */ +LCM_API_FUNCTION +int lcm_subscription_set_queue_capacity(lcm_subscription_t *handler, + int num_messages); + +#define LCM_MAJOR_VERSION 1 +#define LCM_MINOR_VERSION 0 +#define LCM_MICRO_VERSION 0 + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#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 @@ +#ifndef _LCM_LIB_INLINE_H +#define _LCM_LIB_INLINE_H + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +union float_uint32 { + float f; + uint32_t i; +}; + +union double_uint64 { + double f; + uint64_t i; +}; + +typedef struct ___lcm_hash_ptr __lcm_hash_ptr; +struct ___lcm_hash_ptr { + const __lcm_hash_ptr *parent; + void *v; +}; + +/** + * BOOLEAN + */ +#define __boolean_hash_recursive __int8_t_hash_recursive +#define __boolean_decode_array_cleanup __int8_t_decode_array_cleanup +#define __boolean_encoded_array_size __int8_t_encoded_array_size +#define __boolean_encode_array __int8_t_encode_array +#define __boolean_decode_array __int8_t_decode_array +#define __boolean_clone_array __int8_t_clone_array +#define boolean_encoded_size int8_t_encoded_size + +/** + * BYTE + */ +#define __byte_hash_recursive(p) 0 +#define __byte_decode_array_cleanup(p, sz) \ + {} +#define byte_encoded_size(p) (sizeof(int64_t) + sizeof(uint8_t)) + +static inline int __byte_encoded_array_size(const uint8_t *p, int elements) { + (void)p; + return sizeof(uint8_t) * elements; +} + +static inline int __byte_encode_array(void *_buf, int offset, int maxlen, + const uint8_t *p, int elements) { + if (maxlen < elements) + return -1; + + uint8_t *buf = (uint8_t *)_buf; + memcpy(&buf[offset], p, elements); + + return elements; +} + +static inline int __byte_decode_array(const void *_buf, int offset, int maxlen, + uint8_t *p, int elements) { + if (maxlen < elements) + return -1; + + uint8_t *buf = (uint8_t *)_buf; + memcpy(p, &buf[offset], elements); + + return elements; +} + +static inline int __byte_clone_array(const uint8_t *p, uint8_t *q, + int elements) { + memcpy(q, p, elements * sizeof(uint8_t)); + return 0; +} +/** + * INT8_T + */ +#define __int8_t_hash_recursive(p) 0 +#define __int8_t_decode_array_cleanup(p, sz) \ + {} +#define int8_t_encoded_size(p) (sizeof(int64_t) + sizeof(int8_t)) + +static inline int __int8_t_encoded_array_size(const int8_t *p, int elements) { + (void)p; + return sizeof(int8_t) * elements; +} + +static inline int __int8_t_encode_array(void *_buf, int offset, int maxlen, + const int8_t *p, int elements) { + if (maxlen < elements) + return -1; + + int8_t *buf = (int8_t *)_buf; + memcpy(&buf[offset], p, elements); + + return elements; +} + +static inline int __int8_t_decode_array(const void *_buf, int offset, + int maxlen, int8_t *p, int elements) { + if (maxlen < elements) + return -1; + + int8_t *buf = (int8_t *)_buf; + memcpy(p, &buf[offset], elements); + + return elements; +} + +static inline int __int8_t_clone_array(const int8_t *p, int8_t *q, + int elements) { + memcpy(q, p, elements * sizeof(int8_t)); + return 0; +} + +/** + * INT16_T + */ +#define __int16_t_hash_recursive(p) 0 +#define __int16_t_decode_array_cleanup(p, sz) \ + {} +#define int16_t_encoded_size(p) (sizeof(int64_t) + sizeof(int16_t)) + +static inline int __int16_t_encoded_array_size(const int16_t *p, int elements) { + (void)p; + return sizeof(int16_t) * elements; +} + +static inline int __int16_t_encode_array(void *_buf, int offset, int maxlen, + const int16_t *p, int elements) { + int total_size = sizeof(int16_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + int16_t v = p[element]; + buf[pos++] = (v >> 8) & 0xff; + buf[pos++] = (v & 0xff); + } + + return total_size; +} + +static inline int __int16_t_decode_array(const void *_buf, int offset, + int maxlen, int16_t *p, int elements) { + int total_size = sizeof(int16_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + p[element] = (buf[pos] << 8) + buf[pos + 1]; + pos += 2; + } + + return total_size; +} + +static inline int __int16_t_clone_array(const int16_t *p, int16_t *q, + int elements) { + memcpy(q, p, elements * sizeof(int16_t)); + return 0; +} + +/** + * INT32_T + */ +#define __int32_t_hash_recursive(p) 0 +#define __int32_t_decode_array_cleanup(p, sz) \ + {} +#define int32_t_encoded_size(p) (sizeof(int64_t) + sizeof(int32_t)) + +static inline int __int32_t_encoded_array_size(const int32_t *p, int elements) { + (void)p; + return sizeof(int32_t) * elements; +} + +static inline int __int32_t_encode_array(void *_buf, int offset, int maxlen, + const int32_t *p, int elements) { + int total_size = sizeof(int32_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + int32_t v = p[element]; + buf[pos++] = (v >> 24) & 0xff; + buf[pos++] = (v >> 16) & 0xff; + buf[pos++] = (v >> 8) & 0xff; + buf[pos++] = (v & 0xff); + } + + return total_size; +} + +static inline int __int32_t_decode_array(const void *_buf, int offset, + int maxlen, int32_t *p, int elements) { + int total_size = sizeof(int32_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + p[element] = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) + + (buf[pos + 2] << 8) + buf[pos + 3]; + pos += 4; + } + + return total_size; +} + +static inline int __int32_t_clone_array(const int32_t *p, int32_t *q, + int elements) { + memcpy(q, p, elements * sizeof(int32_t)); + return 0; +} + +/** + * INT64_T + */ +#define __int64_t_hash_recursive(p) 0 +#define __int64_t_decode_array_cleanup(p, sz) \ + {} +#define int64_t_encoded_size(p) (sizeof(int64_t) + sizeof(int64_t)) + +static inline int __int64_t_encoded_array_size(const int64_t *p, int elements) { + (void)p; + return sizeof(int64_t) * elements; +} + +static inline int __int64_t_encode_array(void *_buf, int offset, int maxlen, + const int64_t *p, int elements) { + int total_size = sizeof(int64_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + int64_t v = p[element]; + buf[pos++] = (v >> 56) & 0xff; + buf[pos++] = (v >> 48) & 0xff; + buf[pos++] = (v >> 40) & 0xff; + buf[pos++] = (v >> 32) & 0xff; + buf[pos++] = (v >> 24) & 0xff; + buf[pos++] = (v >> 16) & 0xff; + buf[pos++] = (v >> 8) & 0xff; + buf[pos++] = (v & 0xff); + } + + return total_size; +} + +static inline int __int64_t_decode_array(const void *_buf, int offset, + int maxlen, int64_t *p, int elements) { + int total_size = sizeof(int64_t) * elements; + uint8_t *buf = (uint8_t *)_buf; + int pos = offset; + int element; + + if (maxlen < total_size) + return -1; + + for (element = 0; element < elements; element++) { + int64_t a = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) + + (buf[pos + 2] << 8) + buf[pos + 3]; + pos += 4; + int64_t b = (buf[pos + 0] << 24) + (buf[pos + 1] << 16) + + (buf[pos + 2] << 8) + buf[pos + 3]; + pos += 4; + p[element] = (a << 32) + (b & 0xffffffff); + } + + return total_size; +} + +static inline int __int64_t_clone_array(const int64_t *p, int64_t *q, + int elements) { + memcpy(q, p, elements * sizeof(int64_t)); + return 0; +} + +/** + * FLOAT + */ +#define __float_hash_recursive(p) 0 +#define __float_decode_array_cleanup(p, sz) \ + {} +#define float_encoded_size(p) (sizeof(int64_t) + sizeof(float)) + +static inline int __float_encoded_array_size(const float *p, int elements) { + (void)p; + return sizeof(float) * elements; +} + +static inline int __float_encode_array(void *_buf, int offset, int maxlen, + const float *p, int elements) { + return __int32_t_encode_array(_buf, offset, maxlen, (int32_t *)p, elements); +} + +static inline int __float_decode_array(const void *_buf, int offset, int maxlen, + float *p, int elements) { + return __int32_t_decode_array(_buf, offset, maxlen, (int32_t *)p, elements); +} + +static inline int __float_clone_array(const float *p, float *q, int elements) { + memcpy(q, p, elements * sizeof(float)); + return 0; +} + +/** + * DOUBLE + */ +#define __double_hash_recursive(p) 0 +#define __double_decode_array_cleanup(p, sz) \ + {} +#define double_encoded_size(p) (sizeof(int64_t) + sizeof(double)) + +static inline int __double_encoded_array_size(const double *p, int elements) { + (void)p; + return sizeof(double) * elements; +} + +static inline int __double_encode_array(void *_buf, int offset, int maxlen, + const double *p, int elements) { + return __int64_t_encode_array(_buf, offset, maxlen, (int64_t *)p, elements); +} + +static inline int __double_decode_array(const void *_buf, int offset, + int maxlen, double *p, int elements) { + return __int64_t_decode_array(_buf, offset, maxlen, (int64_t *)p, elements); +} + +static inline int __double_clone_array(const double *p, double *q, + int elements) { + memcpy(q, p, elements * sizeof(double)); + return 0; +} + +/** + * STRING + */ +#define __string_hash_recursive(p) 0 + +static inline int __string_decode_array_cleanup(char **s, int elements) { + int element; + for (element = 0; element < elements; element++) + free(s[element]); + return 0; +} + +static inline int __string_encoded_array_size(char *const *s, int elements) { + int size = 0; + int element; + for (element = 0; element < elements; element++) + size += 4 + strlen(s[element]) + 1; + + return size; +} + +static inline int __string_encoded_size(char *const *s) { + return sizeof(int64_t) + __string_encoded_array_size(s, 1); +} + +static inline int __string_encode_array(void *_buf, int offset, int maxlen, + char *const *p, int elements) { + int pos = 0, thislen; + int element; + + for (element = 0; element < elements; element++) { + int length = strlen(p[element]) + 1; // length includes \0 + + thislen = __int32_t_encode_array(_buf, offset + pos, maxlen - pos, + &length, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(_buf, offset + pos, maxlen - pos, + (int8_t *)p[element], length); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + + return pos; +} + +static inline int __string_decode_array(const void *_buf, int offset, + int maxlen, char **p, int elements) { + int pos = 0, thislen; + int element; + + for (element = 0; element < elements; element++) { + int length; + + // read length including \0 + thislen = __int32_t_decode_array(_buf, offset + pos, maxlen - pos, + &length, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element] = (char *)malloc(length); + thislen = __int8_t_decode_array(_buf, offset + pos, maxlen - pos, + (int8_t *)p[element], length); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + + return pos; +} + +static inline int __string_clone_array(char *const *p, char **q, int elements) { + int element; + for (element = 0; element < elements; element++) { + // because strdup is not C99 + size_t len = strlen(p[element]) + 1; + q[element] = (char *)malloc(len); + memcpy(q[element], p[element], len); + } + return 0; +} + +static inline void *lcm_malloc(size_t sz) { + if (sz) + return malloc(sz); + return NULL; +} + +#ifdef __cplusplus +} +#endif + +#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 @@ +#ifndef _LCMTYPE_H +#define _LCMTYPE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "lcm.h" +#include "lcm_coretypes.h" +#ifdef __cplusplus +} +#endif //__cplusplus + +#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 @@ +################################################################################ +# lcmtype.mk +################################################################################ +# set compail dir +MOD_DIR_NAME = lcmtype_dir +MOD_SRC_DIR = $(KERNEL_TOP_DIR)/$(MOD_DIR_NAME) +MOD_OBJ_DIR = ./project/$(MOD_DIR_NAME) + +# get files table +ROBOT_KERNEL_OBJ += $(addprefix $(MOD_OBJ_DIR)/,$(subst .c,.o,$(wildcard $(MOD_SRC_DIR)/*.c))) +ROBOT_KERNEL_FILE_DIR += $(wildcard $(MOD_SRC_DIR)/*.*~) + +# compail +$(MOD_OBJ_DIR)/%.o: $(MOD_SRC_DIR)/%.c + @echo 'Compail... $<' + $(CC) -c $< -o $@ $(C_OPTIMIZATION) $(C_FLAG) $(USR_DEF) + @echo ' ' + +#$(MOD_OBJ_DIR)/%.o: $(MOD_SRC_DIR)/%.c +# @echo 'Compail... $<' +# $(CC) -c $< -o $@ $(C_OPTIMIZATION) $(C_FLAG) $(USR_DEF) +# @echo ' ' diff --git a/lcmtype/path_ctrl.lcm b/lcmtype/path_ctrl.lcm new file mode 100644 index 0000000..502e342 --- /dev/null +++ b/lcmtype/path_ctrl.lcm @@ -0,0 +1,5 @@ +struct path_ctrl_t +{ + int8_t cmd; + int8_t speed; +} diff --git a/lcmtype/path_ctrl_t.c b/lcmtype/path_ctrl_t.c new file mode 100644 index 0000000..2298a56 --- /dev/null +++ b/lcmtype/path_ctrl_t.c @@ -0,0 +1,241 @@ +// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY +// BY HAND!! +// +// Generated by lcm-gen + +#include +#include "path_ctrl_t.h" + +static int __path_ctrl_t_hash_computed; +static uint64_t __path_ctrl_t_hash; + +uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p) +{ + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __path_ctrl_t_get_hash) + return 0; + + __lcm_hash_ptr cp; + cp.parent = p; + cp.v = (void*)__path_ctrl_t_get_hash; + (void) cp; + + uint64_t hash = (uint64_t)0x5cf946463c463b9eLL + + __int8_t_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + ; + + return (hash<<1) + ((hash>>63)&1); +} + +int64_t __path_ctrl_t_get_hash(void) +{ + if (!__path_ctrl_t_hash_computed) { + __path_ctrl_t_hash = (int64_t)__path_ctrl_t_hash_recursive(NULL); + __path_ctrl_t_hash_computed = 1; + } + + return __path_ctrl_t_hash; +} + +int __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements) +{ + int pos = 0, element; + int thislen; + + for (element = 0; element < elements; element++) { + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1); + if (thislen < 0) return thislen; else pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1); + if (thislen < 0) return thislen; else pos += thislen; + + } + return pos; +} + +int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p) +{ + int pos = 0, thislen; + int64_t hash = __path_ctrl_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) return thislen; else pos += thislen; + + thislen = __path_ctrl_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) return thislen; else pos += thislen; + + return pos; +} + +int __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements) +{ + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int8_t_encoded_array_size(&(p[element].cmd), 1); + + size += __int8_t_encoded_array_size(&(p[element].speed), 1); + + } + return size; +} + +int path_ctrl_t_encoded_size(const path_ctrl_t *p) +{ + return 8 + __path_ctrl_t_encoded_array_size(p, 1); +} + +int __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements) +{ + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1); + if (thislen < 0) return thislen; else pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1); + if (thislen < 0) return thislen; else pos += thislen; + + } + return pos; +} + +int __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements) +{ + int element; + for (element = 0; element < elements; element++) { + + __int8_t_decode_array_cleanup(&(p[element].cmd), 1); + + __int8_t_decode_array_cleanup(&(p[element].speed), 1); + + } + return 0; +} + +int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *p) +{ + int pos = 0, thislen; + int64_t hash = __path_ctrl_t_get_hash(); + + int64_t this_hash; + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) return thislen; else pos += thislen; + if (this_hash != hash) return -1; + + thislen = __path_ctrl_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) return thislen; else pos += thislen; + + return pos; +} + +int path_ctrl_t_decode_cleanup(path_ctrl_t *p) +{ + return __path_ctrl_t_decode_array_cleanup(p, 1); +} + +int __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements) +{ + int element; + for (element = 0; element < elements; element++) { + + __int8_t_clone_array(&(p[element].cmd), &(q[element].cmd), 1); + + __int8_t_clone_array(&(p[element].speed), &(q[element].speed), 1); + + } + return 0; +} + +path_ctrl_t *path_ctrl_t_copy(const path_ctrl_t *p) +{ + path_ctrl_t *q = (path_ctrl_t*) malloc(sizeof(path_ctrl_t)); + __path_ctrl_t_clone_array(p, q, 1); + return q; +} + +void path_ctrl_t_destroy(path_ctrl_t *p) +{ + __path_ctrl_t_decode_array_cleanup(p, 1); + free(p); +} + +int path_ctrl_t_publish(lcm_t *lc, const char *channel, const path_ctrl_t *p) +{ + int max_data_size = path_ctrl_t_encoded_size (p); + uint8_t *buf = (uint8_t*) malloc (max_data_size); + if (!buf) return -1; + int data_size = path_ctrl_t_encode (buf, 0, max_data_size, p); + if (data_size < 0) { + free (buf); + return data_size; + } + int status = lcm_publish (lc, channel, buf, data_size); + free (buf); + return status; +} + +struct _path_ctrl_t_subscription_t { + path_ctrl_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static +void path_ctrl_t_handler_stub (const lcm_recv_buf_t *rbuf, + const char *channel, void *userdata) +{ + int status; + path_ctrl_t p; + memset(&p, 0, sizeof(path_ctrl_t)); + status = path_ctrl_t_decode (rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf (stderr, "error %d decoding path_ctrl_t!!!\n", status); + return; + } + + path_ctrl_t_subscription_t *h = (path_ctrl_t_subscription_t*) userdata; + h->user_handler (rbuf, channel, &p, h->userdata); + + path_ctrl_t_decode_cleanup (&p); +} + +path_ctrl_t_subscription_t* path_ctrl_t_subscribe (lcm_t *lcm, + const char *channel, + path_ctrl_t_handler_t f, void *userdata) +{ + path_ctrl_t_subscription_t *n = (path_ctrl_t_subscription_t*) + malloc(sizeof(path_ctrl_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe (lcm, channel, + path_ctrl_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf (stderr,"couldn't reg path_ctrl_t LCM handler!\n"); + free (n); + return NULL; + } + return n; +} + +int path_ctrl_t_subscription_set_queue_capacity (path_ctrl_t_subscription_t* subs, + int num_messages) +{ + return lcm_subscription_set_queue_capacity (subs->lc_h, num_messages); +} + +int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid) +{ + int status = lcm_unsubscribe (lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, + "couldn't unsubscribe path_ctrl_t_handler %p!\n", hid); + return -1; + } + free (hid); + return 0; +} + diff --git a/lcmtype/path_ctrl_t.h b/lcmtype/path_ctrl_t.h new file mode 100644 index 0000000..052ccbc --- /dev/null +++ b/lcmtype/path_ctrl_t.h @@ -0,0 +1,142 @@ +// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY +// BY HAND!! +// +// Generated by lcm-gen + +#include +#include +#include "lcmtype.h" + +#ifndef _path_ctrl_t_h +#define _path_ctrl_t_h + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _path_ctrl_t path_ctrl_t; +struct _path_ctrl_t +{ + int8_t cmd; + int8_t speed; +}; + +/** + * Create a deep copy of a path_ctrl_t. + * When no longer needed, destroy it with path_ctrl_t_destroy() + */ +path_ctrl_t* path_ctrl_t_copy(const path_ctrl_t* to_copy); + +/** + * Destroy an instance of path_ctrl_t created by path_ctrl_t_copy() + */ +void path_ctrl_t_destroy(path_ctrl_t* to_destroy); + +/** + * Identifies a single subscription. This is an opaque data type. + */ +typedef struct _path_ctrl_t_subscription_t path_ctrl_t_subscription_t; + +/** + * Prototype for a callback function invoked when a message of type + * path_ctrl_t is received. + */ +typedef void(*path_ctrl_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, const path_ctrl_t *msg, void *userdata); + +/** + * Publish a message of type path_ctrl_t using LCM. + * + * @param lcm The LCM instance to publish with. + * @param channel The channel to publish on. + * @param msg The message to publish. + * @return 0 on success, <0 on error. Success means LCM has transferred + * responsibility of the message data to the OS. + */ +int path_ctrl_t_publish(lcm_t *lcm, const char *channel, const path_ctrl_t *msg); + +/** + * Subscribe to messages of type path_ctrl_t using LCM. + * + * @param lcm The LCM instance to subscribe with. + * @param channel The channel to subscribe to. + * @param handler The callback function invoked by LCM when a message is received. + * This function is invoked by LCM during calls to lcm_handle() and + * lcm_handle_timeout(). + * @param userdata An opaque pointer passed to @p handler when it is invoked. + * @return 0 on success, <0 if an error occured + */ +path_ctrl_t_subscription_t* path_ctrl_t_subscribe(lcm_t *lcm, const char *channel, path_ctrl_t_handler_t handler, void *userdata); + +/** + * Removes and destroys a subscription created by path_ctrl_t_subscribe() + */ +int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid); + +/** + * Sets the queue capacity for a subscription. + * Some LCM providers (e.g., the default multicast provider) are implemented + * using a background receive thread that constantly revceives messages from + * the network. As these messages are received, they are buffered on + * per-subscription queues until dispatched by lcm_handle(). This function + * how many messages are queued before dropping messages. + * + * @param subs the subscription to modify. + * @param num_messages The maximum number of messages to queue + * on the subscription. + * @return 0 on success, <0 if an error occured + */ +int path_ctrl_t_subscription_set_queue_capacity(path_ctrl_t_subscription_t* subs, + int num_messages); + +/** + * Encode a message of type path_ctrl_t into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at this byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally + * be equal to path_ctrl_t_encoded_size(). + * @param msg The message to encode. + * @return The number of bytes encoded, or <0 if an error occured. + */ +int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p); + +/** + * Decode a message of type path_ctrl_t from binary form. + * When decoding messages containing strings or variable-length arrays, this + * function may allocate memory. When finished with the decoded message, + * release allocated resources with path_ctrl_t_decode_cleanup(). + * + * @param buf The buffer containing the encoded message + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @param msg Output parameter where the decoded message is stored + * @return The number of bytes decoded, or <0 if an error occured. + */ +int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *msg); + +/** + * Release resources allocated by path_ctrl_t_decode() + * @return 0 + */ +int path_ctrl_t_decode_cleanup(path_ctrl_t *p); + +/** + * Check how many bytes are required to encode a message of type path_ctrl_t + */ +int path_ctrl_t_encoded_size(const path_ctrl_t *p); + +// LCM support functions. Users should not call these +int64_t __path_ctrl_t_get_hash(void); +uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p); +int __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements); +int __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements); +int __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements); +int __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements); +int __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements); + +#ifdef __cplusplus +} +#endif + +#endif 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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "path_t.h" +#include + +static int __path_t_hash_computed; +static int64_t __path_t_hash; + +int64_t __path_t_hash_recursive(const __lcm_hash_ptr *p) { + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __path_t_get_hash) + return 0; + + const __lcm_hash_ptr cp = {p, (void *)__path_t_get_hash}; + (void)cp; + + int64_t hash = 0xecdb2a054c696658LL + __int64_t_hash_recursive(&cp) + + __int16_t_hash_recursive(&cp) + __double_hash_recursive(&cp); + + return (hash << 1) + ((hash >> 63) & 1); +} + +int64_t __path_t_get_hash(void) { + if (!__path_t_hash_computed) { + __path_t_hash = __path_t_hash_recursive(NULL); + __path_t_hash_computed = 1; + } + + return __path_t_hash; +} + +int __path_t_encode_array(void *buf, int offset, int maxlen, const path_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int16_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].length), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + { + int a; + for (a = 0; a < p[element].length; a++) { + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].xyr[a], 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + } + } + return pos; +} + +int path_t_encode(void *buf, int offset, int maxlen, const path_t *p) { + int pos = 0, thislen; + int64_t hash = __path_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __path_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int __path_t_encoded_array_size(const path_t *p, int elements) { + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int64_t_encoded_array_size(&(p[element].utime), 1); + + size += __int16_t_encoded_array_size(&(p[element].length), 1); + + { + int a; + for (a = 0; a < p[element].length; a++) { + size += __double_encoded_array_size(p[element].xyr[a], 3); + } + } + } + return size; +} + +int path_t_encoded_size(const path_t *p) { + return 8 + __path_t_encoded_array_size(p, 1); +} + +int __path_t_decode_array(const void *buf, int offset, int maxlen, path_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int16_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].length), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].xyr = + (double **)lcm_malloc(sizeof(double *) * p[element].length); + { + int a; + for (a = 0; a < p[element].length; a++) { + p[element].xyr[a] = (double *)lcm_malloc(sizeof(double) * 3); + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].xyr[a], 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + } + } + return pos; +} + +int __path_t_decode_array_cleanup(path_t *p, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_decode_array_cleanup(&(p[element].utime), 1); + + __int16_t_decode_array_cleanup(&(p[element].length), 1); + + { + int a; + for (a = 0; a < p[element].length; a++) { + __double_decode_array_cleanup(p[element].xyr[a], 3); + if (p[element].xyr[a]) + free(p[element].xyr[a]); + } + } + if (p[element].xyr) + free(p[element].xyr); + } + return 0; +} + +int path_t_decode(const void *buf, int offset, int maxlen, path_t *p) { + int pos = 0, thislen; + int64_t hash = __path_t_get_hash(); + + int64_t this_hash; + thislen = + __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + if (this_hash != hash) + return -1; + + thislen = __path_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int path_t_decode_cleanup(path_t *p) { + return __path_t_decode_array_cleanup(p, 1); +} + +int __path_t_clone_array(const path_t *p, path_t *q, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1); + + __int16_t_clone_array(&(p[element].length), &(q[element].length), 1); + + q[element].xyr = + (double **)lcm_malloc(sizeof(double *) * q[element].length); + { + int a; + for (a = 0; a < p[element].length; a++) { + q[element].xyr[a] = (double *)lcm_malloc(sizeof(double) * 3); + __double_clone_array(p[element].xyr[a], q[element].xyr[a], 3); + } + } + } + return 0; +} + +path_t *path_t_copy(const path_t *p) { + path_t *q = (path_t *)malloc(sizeof(path_t)); + __path_t_clone_array(p, q, 1); + return q; +} + +void path_t_destroy(path_t *p) { + __path_t_decode_array_cleanup(p, 1); + free(p); +} + +int path_t_publish(lcm_t *lc, const char *channel, const path_t *p) { + int max_data_size = path_t_encoded_size(p); + uint8_t *buf = (uint8_t *)malloc(max_data_size); + if (!buf) + return -1; + int data_size = path_t_encode(buf, 0, max_data_size, p); + if (data_size < 0) { + free(buf); + return data_size; + } + int status = lcm_publish(lc, channel, buf, data_size); + free(buf); + return status; +} + +struct _path_t_subscription_t { + path_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static void path_t_handler_stub(const lcm_recv_buf_t *rbuf, const char *channel, + void *userdata) { + int status; + path_t p; + memset(&p, 0, sizeof(path_t)); + status = path_t_decode(rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf(stderr, "error %d decoding path_t!!!\n", status); + return; + } + + path_t_subscription_t *h = (path_t_subscription_t *)userdata; + h->user_handler(rbuf, channel, &p, h->userdata); + + path_t_decode_cleanup(&p); +} + +path_t_subscription_t *path_t_subscribe(lcm_t *lcm, const char *channel, + path_t_handler_t f, void *userdata) { + path_t_subscription_t *n = + (path_t_subscription_t *)malloc(sizeof(path_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe(lcm, channel, path_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf(stderr, "couldn't reg path_t LCM handler!\n"); + free(n); + return NULL; + } + return n; +} + +int path_t_subscription_set_queue_capacity(path_t_subscription_t *subs, + int num_messages) { + return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages); +} + +int path_t_unsubscribe(lcm_t *lcm, path_t_subscription_t *hid) { + int status = lcm_unsubscribe(lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, "couldn't unsubscribe path_t_handler %p!\n", hid); + return -1; + } + free(hid); + return 0; +} 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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "lcmtype.h" +#include +#include + +#ifndef _path_t_h +#define _path_t_h + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _path_t path_t; +struct _path_t { + int64_t utime; + int16_t length; + double **xyr; +}; + +path_t *path_t_copy(const path_t *p); +void path_t_destroy(path_t *p); + +typedef struct _path_t_subscription_t path_t_subscription_t; +typedef void (*path_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, const path_t *msg, + void *user); + +int path_t_publish(lcm_t *lcm, const char *channel, const path_t *p); +path_t_subscription_t *path_t_subscribe(lcm_t *lcm, const char *channel, + path_t_handler_t f, void *userdata); +int path_t_unsubscribe(lcm_t *lcm, path_t_subscription_t *hid); +int path_t_subscription_set_queue_capacity(path_t_subscription_t *subs, + int num_messages); + +int path_t_encode(void *buf, int offset, int maxlen, const path_t *p); +int path_t_decode(const void *buf, int offset, int maxlen, path_t *p); +int path_t_decode_cleanup(path_t *p); +int path_t_encoded_size(const path_t *p); + +// LCM support functions. Users should not call these +int64_t __path_t_get_hash(void); +int64_t __path_t_hash_recursive(const __lcm_hash_ptr *p); +int __path_t_encode_array(void *buf, int offset, int maxlen, const path_t *p, + int elements); +int __path_t_decode_array(const void *buf, int offset, int maxlen, path_t *p, + int elements); +int __path_t_decode_array_cleanup(path_t *p, int elements); +int __path_t_encoded_array_size(const path_t *p, int elements); +int __path_t_clone_array(const path_t *p, path_t *q, int elements); + +#ifdef __cplusplus +} +#endif + +#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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "pose_t.h" +#include + +static int __pose_t_hash_computed; +static int64_t __pose_t_hash; + +int64_t __pose_t_hash_recursive(const __lcm_hash_ptr *p) { + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __pose_t_get_hash) + return 0; + + const __lcm_hash_ptr cp = {p, (void *)__pose_t_get_hash}; + (void)cp; + + int64_t hash = 0x170b77d82958082fLL + __int64_t_hash_recursive(&cp) + + __double_hash_recursive(&cp) + __double_hash_recursive(&cp) + + __double_hash_recursive(&cp) + __double_hash_recursive(&cp) + + __double_hash_recursive(&cp); + + return (hash << 1) + ((hash >> 63) & 1); +} + +int64_t __pose_t_get_hash(void) { + if (!__pose_t_hash_computed) { + __pose_t_hash = __pose_t_hash_recursive(NULL); + __pose_t_hash_computed = 1; + } + + return __pose_t_hash; +} + +int __pose_t_encode_array(void *buf, int offset, int maxlen, const pose_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].pos, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].vel, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].orientation, 4); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].rotation_rate, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].accel, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int pose_t_encode(void *buf, int offset, int maxlen, const pose_t *p) { + int pos = 0, thislen; + int64_t hash = __pose_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __pose_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int __pose_t_encoded_array_size(const pose_t *p, int elements) { + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int64_t_encoded_array_size(&(p[element].utime), 1); + + size += __double_encoded_array_size(p[element].pos, 3); + + size += __double_encoded_array_size(p[element].vel, 3); + + size += __double_encoded_array_size(p[element].orientation, 4); + + size += __double_encoded_array_size(p[element].rotation_rate, 3); + + size += __double_encoded_array_size(p[element].accel, 3); + } + return size; +} + +int pose_t_encoded_size(const pose_t *p) { + return 8 + __pose_t_encoded_array_size(p, 1); +} + +int __pose_t_decode_array(const void *buf, int offset, int maxlen, pose_t *p, + int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].pos, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].vel, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].orientation, 4); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].rotation_rate, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].accel, 3); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int __pose_t_decode_array_cleanup(pose_t *p, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_decode_array_cleanup(&(p[element].utime), 1); + + __double_decode_array_cleanup(p[element].pos, 3); + + __double_decode_array_cleanup(p[element].vel, 3); + + __double_decode_array_cleanup(p[element].orientation, 4); + + __double_decode_array_cleanup(p[element].rotation_rate, 3); + + __double_decode_array_cleanup(p[element].accel, 3); + } + return 0; +} + +int pose_t_decode(const void *buf, int offset, int maxlen, pose_t *p) { + int pos = 0, thislen; + int64_t hash = __pose_t_get_hash(); + + int64_t this_hash; + thislen = + __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + if (this_hash != hash) + return -1; + + thislen = __pose_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int pose_t_decode_cleanup(pose_t *p) { + return __pose_t_decode_array_cleanup(p, 1); +} + +int __pose_t_clone_array(const pose_t *p, pose_t *q, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1); + + __double_clone_array(p[element].pos, q[element].pos, 3); + + __double_clone_array(p[element].vel, q[element].vel, 3); + + __double_clone_array(p[element].orientation, q[element].orientation, 4); + + __double_clone_array(p[element].rotation_rate, q[element].rotation_rate, + 3); + + __double_clone_array(p[element].accel, q[element].accel, 3); + } + return 0; +} + +pose_t *pose_t_copy(const pose_t *p) { + pose_t *q = (pose_t *)malloc(sizeof(pose_t)); + __pose_t_clone_array(p, q, 1); + return q; +} + +void pose_t_destroy(pose_t *p) { + __pose_t_decode_array_cleanup(p, 1); + free(p); +} + +int pose_t_publish(lcm_t *lc, const char *channel, const pose_t *p) { + int max_data_size = pose_t_encoded_size(p); + uint8_t *buf = (uint8_t *)malloc(max_data_size); + if (!buf) + return -1; + int data_size = pose_t_encode(buf, 0, max_data_size, p); + if (data_size < 0) { + free(buf); + return data_size; + } + int status = lcm_publish(lc, channel, buf, data_size); + free(buf); + return status; +} + +struct _pose_t_subscription_t { + pose_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static void pose_t_handler_stub(const lcm_recv_buf_t *rbuf, const char *channel, + void *userdata) { + int status; + pose_t p; + memset(&p, 0, sizeof(pose_t)); + status = pose_t_decode(rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf(stderr, "error %d decoding pose_t!!!\n", status); + return; + } + + pose_t_subscription_t *h = (pose_t_subscription_t *)userdata; + h->user_handler(rbuf, channel, &p, h->userdata); + + pose_t_decode_cleanup(&p); +} + +pose_t_subscription_t *pose_t_subscribe(lcm_t *lcm, const char *channel, + pose_t_handler_t f, void *userdata) { + pose_t_subscription_t *n = + (pose_t_subscription_t *)malloc(sizeof(pose_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe(lcm, channel, pose_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf(stderr, "couldn't reg pose_t LCM handler!\n"); + free(n); + return NULL; + } + return n; +} + +int pose_t_subscription_set_queue_capacity(pose_t_subscription_t *subs, + int num_messages) { + return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages); +} + +int pose_t_unsubscribe(lcm_t *lcm, pose_t_subscription_t *hid) { + int status = lcm_unsubscribe(lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, "couldn't unsubscribe pose_t_handler %p!\n", hid); + return -1; + } + free(hid); + return 0; +} 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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "lcmtype.h" +#include +#include + +#ifndef _pose_t_h +#define _pose_t_h + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _pose_t pose_t; +struct _pose_t { + int64_t utime; + double pos[3]; + double vel[3]; + double orientation[4]; + double rotation_rate[3]; + double accel[3]; +}; + +pose_t *pose_t_copy(const pose_t *p); +void pose_t_destroy(pose_t *p); + +typedef struct _pose_t_subscription_t pose_t_subscription_t; +typedef void (*pose_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, const pose_t *msg, + void *user); + +int pose_t_publish(lcm_t *lcm, const char *channel, const pose_t *p); +pose_t_subscription_t *pose_t_subscribe(lcm_t *lcm, const char *channel, + pose_t_handler_t f, void *userdata); +int pose_t_unsubscribe(lcm_t *lcm, pose_t_subscription_t *hid); +int pose_t_subscription_set_queue_capacity(pose_t_subscription_t *subs, + int num_messages); + +int pose_t_encode(void *buf, int offset, int maxlen, const pose_t *p); +int pose_t_decode(const void *buf, int offset, int maxlen, pose_t *p); +int pose_t_decode_cleanup(pose_t *p); +int pose_t_encoded_size(const pose_t *p); + +// LCM support functions. Users should not call these +int64_t __pose_t_get_hash(void); +int64_t __pose_t_hash_recursive(const __lcm_hash_ptr *p); +int __pose_t_encode_array(void *buf, int offset, int maxlen, const pose_t *p, + int elements); +int __pose_t_decode_array(const void *buf, int offset, int maxlen, pose_t *p, + int elements); +int __pose_t_decode_array_cleanup(pose_t *p, int elements); +int __pose_t_encoded_array_size(const pose_t *p, int elements); +int __pose_t_clone_array(const pose_t *p, pose_t *q, int elements); + +#ifdef __cplusplus +} +#endif + +#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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "robot_control_t.h" +#include + +static int __robot_control_t_hash_computed; +static int64_t __robot_control_t_hash; + +int64_t __robot_control_t_hash_recursive(const __lcm_hash_ptr *p) { + const __lcm_hash_ptr *fp; + for (fp = p; fp != NULL; fp = fp->parent) + if (fp->v == __robot_control_t_get_hash) + return 0; + + const __lcm_hash_ptr cp = {p, (void *)__robot_control_t_get_hash}; + (void)cp; + + int64_t hash = 0x38f63251f9863f70LL + __int64_t_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + __int8_t_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + __double_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + __int8_t_hash_recursive(&cp) + + __int8_t_hash_recursive(&cp) + __string_hash_recursive(&cp) + + __int64_t_hash_recursive(&cp) + __byte_hash_recursive(&cp); + + return (hash << 1) + ((hash >> 63) & 1); +} + +int64_t __robot_control_t_get_hash(void) { + if (!__robot_control_t_hash_computed) { + __robot_control_t_hash = __robot_control_t_hash_recursive(NULL); + __robot_control_t_hash_computed = 1; + } + + return __robot_control_t_hash; +} + +int __robot_control_t_encode_array(void *buf, int offset, int maxlen, + const robot_control_t *p, int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].commandid), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].robotid), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].ndparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = + __double_encode_array(buf, offset + pos, maxlen - pos, + p[element].dparams, p[element].ndparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].niparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = + __int8_t_encode_array(buf, offset + pos, maxlen - pos, + p[element].iparams, p[element].niparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].nsparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = + __string_encode_array(buf, offset + pos, maxlen - pos, + p[element].sparams, p[element].nsparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, + &(p[element].nbparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __byte_encode_array(buf, offset + pos, maxlen - pos, + p[element].bparams, p[element].nbparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int robot_control_t_encode(void *buf, int offset, int maxlen, + const robot_control_t *p) { + int pos = 0, thislen; + int64_t hash = __robot_control_t_get_hash(); + + thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = + __robot_control_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int __robot_control_t_encoded_array_size(const robot_control_t *p, + int elements) { + int size = 0, element; + for (element = 0; element < elements; element++) { + + size += __int64_t_encoded_array_size(&(p[element].utime), 1); + + size += __int8_t_encoded_array_size(&(p[element].commandid), 1); + + size += __int8_t_encoded_array_size(&(p[element].robotid), 1); + + size += __int8_t_encoded_array_size(&(p[element].ndparams), 1); + + size += __double_encoded_array_size(p[element].dparams, + p[element].ndparams); + + size += __int8_t_encoded_array_size(&(p[element].niparams), 1); + + size += __int8_t_encoded_array_size(p[element].iparams, + p[element].niparams); + + size += __int8_t_encoded_array_size(&(p[element].nsparams), 1); + + size += __string_encoded_array_size(p[element].sparams, + p[element].nsparams); + + size += __int64_t_encoded_array_size(&(p[element].nbparams), 1); + + size += + __byte_encoded_array_size(p[element].bparams, p[element].nbparams); + } + return size; +} + +int robot_control_t_encoded_size(const robot_control_t *p) { + return 8 + __robot_control_t_encoded_array_size(p, 1); +} + +int __robot_control_t_decode_array(const void *buf, int offset, int maxlen, + robot_control_t *p, int elements) { + int pos = 0, thislen, element; + + for (element = 0; element < elements; element++) { + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].utime), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].commandid), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].robotid), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].ndparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].dparams = + (double *)lcm_malloc(sizeof(double) * p[element].ndparams); + thislen = + __double_decode_array(buf, offset + pos, maxlen - pos, + p[element].dparams, p[element].ndparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].niparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].iparams = + (int8_t *)lcm_malloc(sizeof(int8_t) * p[element].niparams); + thislen = + __int8_t_decode_array(buf, offset + pos, maxlen - pos, + p[element].iparams, p[element].niparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].nsparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].sparams = + (char **)lcm_malloc(sizeof(char *) * p[element].nsparams); + thislen = + __string_decode_array(buf, offset + pos, maxlen - pos, + p[element].sparams, p[element].nsparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + + thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, + &(p[element].nbparams), 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + p[element].bparams = + (uint8_t *)lcm_malloc(sizeof(uint8_t) * p[element].nbparams); + thislen = __byte_decode_array(buf, offset + pos, maxlen - pos, + p[element].bparams, p[element].nbparams); + if (thislen < 0) + return thislen; + else + pos += thislen; + } + return pos; +} + +int __robot_control_t_decode_array_cleanup(robot_control_t *p, int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_decode_array_cleanup(&(p[element].utime), 1); + + __int8_t_decode_array_cleanup(&(p[element].commandid), 1); + + __int8_t_decode_array_cleanup(&(p[element].robotid), 1); + + __int8_t_decode_array_cleanup(&(p[element].ndparams), 1); + + __double_decode_array_cleanup(p[element].dparams, p[element].ndparams); + if (p[element].dparams) + free(p[element].dparams); + + __int8_t_decode_array_cleanup(&(p[element].niparams), 1); + + __int8_t_decode_array_cleanup(p[element].iparams, p[element].niparams); + if (p[element].iparams) + free(p[element].iparams); + + __int8_t_decode_array_cleanup(&(p[element].nsparams), 1); + + __string_decode_array_cleanup(p[element].sparams, p[element].nsparams); + if (p[element].sparams) + free(p[element].sparams); + + __int64_t_decode_array_cleanup(&(p[element].nbparams), 1); + + __byte_decode_array_cleanup(p[element].bparams, p[element].nbparams); + if (p[element].bparams) + free(p[element].bparams); + } + return 0; +} + +int robot_control_t_decode(const void *buf, int offset, int maxlen, + robot_control_t *p) { + int pos = 0, thislen; + int64_t hash = __robot_control_t_get_hash(); + + int64_t this_hash; + thislen = + __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + if (this_hash != hash) + return -1; + + thislen = + __robot_control_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); + if (thislen < 0) + return thislen; + else + pos += thislen; + + return pos; +} + +int robot_control_t_decode_cleanup(robot_control_t *p) { + return __robot_control_t_decode_array_cleanup(p, 1); +} + +int __robot_control_t_clone_array(const robot_control_t *p, robot_control_t *q, + int elements) { + int element; + for (element = 0; element < elements; element++) { + + __int64_t_clone_array(&(p[element].utime), &(q[element].utime), 1); + + __int8_t_clone_array(&(p[element].commandid), &(q[element].commandid), + 1); + + __int8_t_clone_array(&(p[element].robotid), &(q[element].robotid), 1); + + __int8_t_clone_array(&(p[element].ndparams), &(q[element].ndparams), 1); + + q[element].dparams = + (double *)lcm_malloc(sizeof(double) * q[element].ndparams); + __double_clone_array(p[element].dparams, q[element].dparams, + p[element].ndparams); + + __int8_t_clone_array(&(p[element].niparams), &(q[element].niparams), 1); + + q[element].iparams = + (int8_t *)lcm_malloc(sizeof(int8_t) * q[element].niparams); + __int8_t_clone_array(p[element].iparams, q[element].iparams, + p[element].niparams); + + __int8_t_clone_array(&(p[element].nsparams), &(q[element].nsparams), 1); + + q[element].sparams = + (char **)lcm_malloc(sizeof(char *) * q[element].nsparams); + __string_clone_array(p[element].sparams, q[element].sparams, + p[element].nsparams); + + __int64_t_clone_array(&(p[element].nbparams), &(q[element].nbparams), + 1); + + q[element].bparams = + (uint8_t *)lcm_malloc(sizeof(uint8_t) * q[element].nbparams); + __byte_clone_array(p[element].bparams, q[element].bparams, + p[element].nbparams); + } + return 0; +} + +robot_control_t *robot_control_t_copy(const robot_control_t *p) { + robot_control_t *q = (robot_control_t *)malloc(sizeof(robot_control_t)); + __robot_control_t_clone_array(p, q, 1); + return q; +} + +void robot_control_t_destroy(robot_control_t *p) { + __robot_control_t_decode_array_cleanup(p, 1); + free(p); +} + +int robot_control_t_publish(lcm_t *lc, const char *channel, + const robot_control_t *p) { + int max_data_size = robot_control_t_encoded_size(p); + uint8_t *buf = (uint8_t *)malloc(max_data_size); + if (!buf) + return -1; + int data_size = robot_control_t_encode(buf, 0, max_data_size, p); + if (data_size < 0) { + free(buf); + return data_size; + } + int status = lcm_publish(lc, channel, buf, data_size); + free(buf); + return status; +} + +struct _robot_control_t_subscription_t { + robot_control_t_handler_t user_handler; + void *userdata; + lcm_subscription_t *lc_h; +}; +static void robot_control_t_handler_stub(const lcm_recv_buf_t *rbuf, + const char *channel, void *userdata) { + int status; + robot_control_t p; + memset(&p, 0, sizeof(robot_control_t)); + status = robot_control_t_decode(rbuf->data, 0, rbuf->data_size, &p); + if (status < 0) { + fprintf(stderr, "error %d decoding robot_control_t!!!\n", status); + return; + } + + robot_control_t_subscription_t *h = + (robot_control_t_subscription_t *)userdata; + h->user_handler(rbuf, channel, &p, h->userdata); + + robot_control_t_decode_cleanup(&p); +} + +robot_control_t_subscription_t * +robot_control_t_subscribe(lcm_t *lcm, const char *channel, + robot_control_t_handler_t f, void *userdata) { + robot_control_t_subscription_t *n = + (robot_control_t_subscription_t *)malloc( + sizeof(robot_control_t_subscription_t)); + n->user_handler = f; + n->userdata = userdata; + n->lc_h = lcm_subscribe(lcm, channel, robot_control_t_handler_stub, n); + if (n->lc_h == NULL) { + fprintf(stderr, "couldn't reg robot_control_t LCM handler!\n"); + free(n); + return NULL; + } + return n; +} + +int robot_control_t_subscription_set_queue_capacity( + robot_control_t_subscription_t *subs, int num_messages) { + return lcm_subscription_set_queue_capacity(subs->lc_h, num_messages); +} + +int robot_control_t_unsubscribe(lcm_t *lcm, + robot_control_t_subscription_t *hid) { + int status = lcm_unsubscribe(lcm, hid->lc_h); + if (0 != status) { + fprintf(stderr, "couldn't unsubscribe robot_control_t_handler %p!\n", + hid); + return -1; + } + free(hid); + return 0; +} 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 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#include "lcmtype.h" +#include +#include + +#ifndef _robot_control_t_h +#define _robot_control_t_h + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _robot_control_t robot_control_t; +struct _robot_control_t { + int64_t utime; + int8_t commandid; + int8_t robotid; + int8_t ndparams; + double *dparams; + int8_t niparams; + int8_t *iparams; + int8_t nsparams; + char **sparams; + int64_t nbparams; + uint8_t *bparams; +}; + +robot_control_t *robot_control_t_copy(const robot_control_t *p); +void robot_control_t_destroy(robot_control_t *p); + +typedef struct _robot_control_t_subscription_t robot_control_t_subscription_t; +typedef void (*robot_control_t_handler_t)(const lcm_recv_buf_t *rbuf, + const char *channel, + const robot_control_t *msg, + void *user); + +int robot_control_t_publish(lcm_t *lcm, const char *channel, + const robot_control_t *p); +robot_control_t_subscription_t * +robot_control_t_subscribe(lcm_t *lcm, const char *channel, + robot_control_t_handler_t f, void *userdata); +int robot_control_t_unsubscribe(lcm_t *lcm, + robot_control_t_subscription_t *hid); +int robot_control_t_subscription_set_queue_capacity( + robot_control_t_subscription_t *subs, int num_messages); + +int robot_control_t_encode(void *buf, int offset, int maxlen, + const robot_control_t *p); +int robot_control_t_decode(const void *buf, int offset, int maxlen, + robot_control_t *p); +int robot_control_t_decode_cleanup(robot_control_t *p); +int robot_control_t_encoded_size(const robot_control_t *p); + +// LCM support functions. Users should not call these +int64_t __robot_control_t_get_hash(void); +int64_t __robot_control_t_hash_recursive(const __lcm_hash_ptr *p); +int __robot_control_t_encode_array(void *buf, int offset, int maxlen, + const robot_control_t *p, int elements); +int __robot_control_t_decode_array(const void *buf, int offset, int maxlen, + robot_control_t *p, int elements); +int __robot_control_t_decode_array_cleanup(robot_control_t *p, int elements); +int __robot_control_t_encoded_array_size(const robot_control_t *p, + int elements); +int __robot_control_t_clone_array(const robot_control_t *p, robot_control_t *q, + int elements); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/lib/aarch64/liblcm.so b/lib/aarch64/liblcm.so new file mode 100644 index 0000000..98bb954 Binary files /dev/null and b/lib/aarch64/liblcm.so differ diff --git a/lib/liblcm.so b/lib/liblcm.so deleted file mode 100755 index cb18db8..0000000 Binary files a/lib/liblcm.so and /dev/null differ diff --git a/lib/x86/liblcm.so b/lib/x86/liblcm.so new file mode 100755 index 0000000..cb18db8 Binary files /dev/null and b/lib/x86/liblcm.so differ diff --git a/path/path_ctrl.lcm b/path/path_ctrl.lcm deleted file mode 100644 index 502e342..0000000 --- a/path/path_ctrl.lcm +++ /dev/null @@ -1,5 +0,0 @@ -struct path_ctrl_t -{ - int8_t cmd; - int8_t speed; -} diff --git a/path/path_ctrl_t.c b/path/path_ctrl_t.c deleted file mode 100644 index 2298a56..0000000 --- a/path/path_ctrl_t.c +++ /dev/null @@ -1,241 +0,0 @@ -// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY -// BY HAND!! -// -// Generated by lcm-gen - -#include -#include "path_ctrl_t.h" - -static int __path_ctrl_t_hash_computed; -static uint64_t __path_ctrl_t_hash; - -uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p) -{ - const __lcm_hash_ptr *fp; - for (fp = p; fp != NULL; fp = fp->parent) - if (fp->v == __path_ctrl_t_get_hash) - return 0; - - __lcm_hash_ptr cp; - cp.parent = p; - cp.v = (void*)__path_ctrl_t_get_hash; - (void) cp; - - uint64_t hash = (uint64_t)0x5cf946463c463b9eLL - + __int8_t_hash_recursive(&cp) - + __int8_t_hash_recursive(&cp) - ; - - return (hash<<1) + ((hash>>63)&1); -} - -int64_t __path_ctrl_t_get_hash(void) -{ - if (!__path_ctrl_t_hash_computed) { - __path_ctrl_t_hash = (int64_t)__path_ctrl_t_hash_recursive(NULL); - __path_ctrl_t_hash_computed = 1; - } - - return __path_ctrl_t_hash; -} - -int __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements) -{ - int pos = 0, element; - int thislen; - - for (element = 0; element < elements; element++) { - - thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1); - if (thislen < 0) return thislen; else pos += thislen; - - thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1); - if (thislen < 0) return thislen; else pos += thislen; - - } - return pos; -} - -int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p) -{ - int pos = 0, thislen; - int64_t hash = __path_ctrl_t_get_hash(); - - thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); - if (thislen < 0) return thislen; else pos += thislen; - - thislen = __path_ctrl_t_encode_array(buf, offset + pos, maxlen - pos, p, 1); - if (thislen < 0) return thislen; else pos += thislen; - - return pos; -} - -int __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements) -{ - int size = 0, element; - for (element = 0; element < elements; element++) { - - size += __int8_t_encoded_array_size(&(p[element].cmd), 1); - - size += __int8_t_encoded_array_size(&(p[element].speed), 1); - - } - return size; -} - -int path_ctrl_t_encoded_size(const path_ctrl_t *p) -{ - return 8 + __path_ctrl_t_encoded_array_size(p, 1); -} - -int __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements) -{ - int pos = 0, thislen, element; - - for (element = 0; element < elements; element++) { - - thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1); - if (thislen < 0) return thislen; else pos += thislen; - - thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1); - if (thislen < 0) return thislen; else pos += thislen; - - } - return pos; -} - -int __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements) -{ - int element; - for (element = 0; element < elements; element++) { - - __int8_t_decode_array_cleanup(&(p[element].cmd), 1); - - __int8_t_decode_array_cleanup(&(p[element].speed), 1); - - } - return 0; -} - -int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *p) -{ - int pos = 0, thislen; - int64_t hash = __path_ctrl_t_get_hash(); - - int64_t this_hash; - thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1); - if (thislen < 0) return thislen; else pos += thislen; - if (this_hash != hash) return -1; - - thislen = __path_ctrl_t_decode_array(buf, offset + pos, maxlen - pos, p, 1); - if (thislen < 0) return thislen; else pos += thislen; - - return pos; -} - -int path_ctrl_t_decode_cleanup(path_ctrl_t *p) -{ - return __path_ctrl_t_decode_array_cleanup(p, 1); -} - -int __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements) -{ - int element; - for (element = 0; element < elements; element++) { - - __int8_t_clone_array(&(p[element].cmd), &(q[element].cmd), 1); - - __int8_t_clone_array(&(p[element].speed), &(q[element].speed), 1); - - } - return 0; -} - -path_ctrl_t *path_ctrl_t_copy(const path_ctrl_t *p) -{ - path_ctrl_t *q = (path_ctrl_t*) malloc(sizeof(path_ctrl_t)); - __path_ctrl_t_clone_array(p, q, 1); - return q; -} - -void path_ctrl_t_destroy(path_ctrl_t *p) -{ - __path_ctrl_t_decode_array_cleanup(p, 1); - free(p); -} - -int path_ctrl_t_publish(lcm_t *lc, const char *channel, const path_ctrl_t *p) -{ - int max_data_size = path_ctrl_t_encoded_size (p); - uint8_t *buf = (uint8_t*) malloc (max_data_size); - if (!buf) return -1; - int data_size = path_ctrl_t_encode (buf, 0, max_data_size, p); - if (data_size < 0) { - free (buf); - return data_size; - } - int status = lcm_publish (lc, channel, buf, data_size); - free (buf); - return status; -} - -struct _path_ctrl_t_subscription_t { - path_ctrl_t_handler_t user_handler; - void *userdata; - lcm_subscription_t *lc_h; -}; -static -void path_ctrl_t_handler_stub (const lcm_recv_buf_t *rbuf, - const char *channel, void *userdata) -{ - int status; - path_ctrl_t p; - memset(&p, 0, sizeof(path_ctrl_t)); - status = path_ctrl_t_decode (rbuf->data, 0, rbuf->data_size, &p); - if (status < 0) { - fprintf (stderr, "error %d decoding path_ctrl_t!!!\n", status); - return; - } - - path_ctrl_t_subscription_t *h = (path_ctrl_t_subscription_t*) userdata; - h->user_handler (rbuf, channel, &p, h->userdata); - - path_ctrl_t_decode_cleanup (&p); -} - -path_ctrl_t_subscription_t* path_ctrl_t_subscribe (lcm_t *lcm, - const char *channel, - path_ctrl_t_handler_t f, void *userdata) -{ - path_ctrl_t_subscription_t *n = (path_ctrl_t_subscription_t*) - malloc(sizeof(path_ctrl_t_subscription_t)); - n->user_handler = f; - n->userdata = userdata; - n->lc_h = lcm_subscribe (lcm, channel, - path_ctrl_t_handler_stub, n); - if (n->lc_h == NULL) { - fprintf (stderr,"couldn't reg path_ctrl_t LCM handler!\n"); - free (n); - return NULL; - } - return n; -} - -int path_ctrl_t_subscription_set_queue_capacity (path_ctrl_t_subscription_t* subs, - int num_messages) -{ - return lcm_subscription_set_queue_capacity (subs->lc_h, num_messages); -} - -int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid) -{ - int status = lcm_unsubscribe (lcm, hid->lc_h); - if (0 != status) { - fprintf(stderr, - "couldn't unsubscribe path_ctrl_t_handler %p!\n", hid); - return -1; - } - free (hid); - return 0; -} - diff --git a/path/path_ctrl_t.h b/path/path_ctrl_t.h deleted file mode 100644 index 3e629eb..0000000 --- a/path/path_ctrl_t.h +++ /dev/null @@ -1,143 +0,0 @@ -// THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY -// BY HAND!! -// -// Generated by lcm-gen - -#include -#include -#include -#include - -#ifndef _path_ctrl_t_h -#define _path_ctrl_t_h - -#ifdef __cplusplus -extern "C" { -#endif - -typedef struct _path_ctrl_t path_ctrl_t; -struct _path_ctrl_t -{ - int8_t cmd; - int8_t speed; -}; - -/** - * Create a deep copy of a path_ctrl_t. - * When no longer needed, destroy it with path_ctrl_t_destroy() - */ -path_ctrl_t* path_ctrl_t_copy(const path_ctrl_t* to_copy); - -/** - * Destroy an instance of path_ctrl_t created by path_ctrl_t_copy() - */ -void path_ctrl_t_destroy(path_ctrl_t* to_destroy); - -/** - * Identifies a single subscription. This is an opaque data type. - */ -typedef struct _path_ctrl_t_subscription_t path_ctrl_t_subscription_t; - -/** - * Prototype for a callback function invoked when a message of type - * path_ctrl_t is received. - */ -typedef void(*path_ctrl_t_handler_t)(const lcm_recv_buf_t *rbuf, - const char *channel, const path_ctrl_t *msg, void *userdata); - -/** - * Publish a message of type path_ctrl_t using LCM. - * - * @param lcm The LCM instance to publish with. - * @param channel The channel to publish on. - * @param msg The message to publish. - * @return 0 on success, <0 on error. Success means LCM has transferred - * responsibility of the message data to the OS. - */ -int path_ctrl_t_publish(lcm_t *lcm, const char *channel, const path_ctrl_t *msg); - -/** - * Subscribe to messages of type path_ctrl_t using LCM. - * - * @param lcm The LCM instance to subscribe with. - * @param channel The channel to subscribe to. - * @param handler The callback function invoked by LCM when a message is received. - * This function is invoked by LCM during calls to lcm_handle() and - * lcm_handle_timeout(). - * @param userdata An opaque pointer passed to @p handler when it is invoked. - * @return 0 on success, <0 if an error occured - */ -path_ctrl_t_subscription_t* path_ctrl_t_subscribe(lcm_t *lcm, const char *channel, path_ctrl_t_handler_t handler, void *userdata); - -/** - * Removes and destroys a subscription created by path_ctrl_t_subscribe() - */ -int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid); - -/** - * Sets the queue capacity for a subscription. - * Some LCM providers (e.g., the default multicast provider) are implemented - * using a background receive thread that constantly revceives messages from - * the network. As these messages are received, they are buffered on - * per-subscription queues until dispatched by lcm_handle(). This function - * how many messages are queued before dropping messages. - * - * @param subs the subscription to modify. - * @param num_messages The maximum number of messages to queue - * on the subscription. - * @return 0 on success, <0 if an error occured - */ -int path_ctrl_t_subscription_set_queue_capacity(path_ctrl_t_subscription_t* subs, - int num_messages); - -/** - * Encode a message of type path_ctrl_t into binary form. - * - * @param buf The output buffer. - * @param offset Encoding starts at this byte offset into @p buf. - * @param maxlen Maximum number of bytes to write. This should generally - * be equal to path_ctrl_t_encoded_size(). - * @param msg The message to encode. - * @return The number of bytes encoded, or <0 if an error occured. - */ -int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p); - -/** - * Decode a message of type path_ctrl_t from binary form. - * When decoding messages containing strings or variable-length arrays, this - * function may allocate memory. When finished with the decoded message, - * release allocated resources with path_ctrl_t_decode_cleanup(). - * - * @param buf The buffer containing the encoded message - * @param offset The byte offset into @p buf where the encoded message starts. - * @param maxlen The maximum number of bytes to read while decoding. - * @param msg Output parameter where the decoded message is stored - * @return The number of bytes decoded, or <0 if an error occured. - */ -int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *msg); - -/** - * Release resources allocated by path_ctrl_t_decode() - * @return 0 - */ -int path_ctrl_t_decode_cleanup(path_ctrl_t *p); - -/** - * Check how many bytes are required to encode a message of type path_ctrl_t - */ -int path_ctrl_t_encoded_size(const path_ctrl_t *p); - -// LCM support functions. Users should not call these -int64_t __path_ctrl_t_get_hash(void); -uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p); -int __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements); -int __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements); -int __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements); -int __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements); -int __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/serial.c b/serial.c deleted file mode 100644 index 6b4eb72..0000000 --- a/serial.c +++ /dev/null @@ -1,130 +0,0 @@ -#include "path/path_ctrl_t.h" -#include -#include -#include -#include -#include -#include -#include -#include - -typedef unsigned char byte; -#define MAX_BUFFER_SIZE 1024 -#define DEFAULT_SPEED 0x15 - -int fd; // 轮子的串口文件描述符 -char portname[50] = "/dev/ttyUSB0"; // 串口设备名 -char clientIP[20] = ""; -struct termios tty; - -bool whellInit(); -bool wheelSend(byte a, byte a_v, byte b, byte b_v); -void speedControl(byte status, byte speed); -void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, - const path_ctrl_t *msg, void *userdata); - -int main() { - if (!whellInit()) { - goto err; - } - lcm_t *path_lcm = lcm_create(NULL); - if (!path_lcm) { - fprintf(stderr, "Failed to create LCM\n"); - goto err; - } - path_ctrl_t cmd; - path_ctrl_t_subscribe(path_lcm, "wheel_ctrl", parseCmd, &cmd); - - while (true) { - lcm_handle(path_lcm); - } - -err: - close(fd); - return 0; -} - -bool whellInit() { - // 打开串口 - fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); - if (fd < 0) { - fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno)); - return false; - } - - // 设置串口参数 - memset(&tty, 0, sizeof tty); - if (tcgetattr(fd, &tty) != 0) { - fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno)); - return false; - } - - cfsetospeed(&tty, B115200); // 设置输出波特率为115200 - cfsetispeed(&tty, B115200); // 设置输入波特率为115200 - - tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars - tty.c_iflag &= ~IGNBRK; // ignore break signal - tty.c_lflag = 0; // no signaling chars, no echo, - // no canonical processing - tty.c_oflag = 0; // no remapping, no delays - tty.c_cc[VMIN] = 0; // read doesn't block - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl - tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, - // enable reading - tty.c_cflag &= ~(PARENB | PARODD); // shut off parity - tty.c_cflag |= 0; - tty.c_cflag &= ~CSTOPB; - tty.c_cflag &= ~CRTSCTS; - - if (tcsetattr(fd, TCSANOW, &tty) != 0) { - fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno)); - return false; - } - return true; -} - -bool wheelSend(byte a, byte a_v, byte b, byte b_v) { - unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}; - byte checksum = 0; - data[2] = a; - data[3] = a_v; - data[4] = b; - data[5] = b_v; - - for (int i = 0; i < 6; i++) { - checksum ^= data[i]; - } - data[6] = checksum; - - // 发送数据 - if (write(fd, data, 7) != 7) { - fprintf(stderr, "Failed to write to the serial port\n"); - return false; - } - - printf("Data %02X %02X %02X %02X %02X %02X %02X wheelSend successfully!\n", - data[0], data[1], data[2], data[3], data[4], data[5], data[6]); - return true; -} - -void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, - const path_ctrl_t *msg, void *userdata) { - byte status = msg->cmd, speed = msg->speed; - switch (status) { - case 1: - wheelSend(0x01, speed, 0x01, speed); - break; - case 2: - wheelSend(0x02, DEFAULT_SPEED, 0x01, DEFAULT_SPEED); - break; - case 3: - wheelSend(0x01, DEFAULT_SPEED, 0x02, DEFAULT_SPEED); - break; - case 0: - default: - wheelSend(0x00, 0x00, 0x00, 0x00); - break; - } -} \ No newline at end of file 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 @@ +cmake_minimum_required(VERSION 3.10) +project(serial) + +add_executable(serial serial.c ${CMAKE_SOURCE_DIR}/lcmtype/path_ctrl_t.c) + +# 添加链接库 +target_link_libraries(serial lcm) \ No newline at end of file diff --git a/serial/serial.c b/serial/serial.c new file mode 100644 index 0000000..db45fac --- /dev/null +++ b/serial/serial.c @@ -0,0 +1,130 @@ +#include "path_ctrl_t.h" +#include +#include +#include +#include +#include +#include +#include +#include + +typedef unsigned char byte; +#define MAX_BUFFER_SIZE 1024 +#define DEFAULT_SPEED 0x15 + +int fd; // 轮子的串口文件描述符 +char portname[50] = "/dev/ttyUSB0"; // 串口设备名 +char clientIP[20] = ""; +struct termios tty; + +bool whellInit(); +bool wheelSend(byte a, byte a_v, byte b, byte b_v); +void speedControl(byte status, byte speed); +void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, + const path_ctrl_t *msg, void *userdata); + +int main() { + if (!whellInit()) { + goto err; + } + lcm_t *path_lcm = lcm_create(NULL); + if (!path_lcm) { + fprintf(stderr, "Failed to create LCM\n"); + goto err; + } + path_ctrl_t cmd; + path_ctrl_t_subscribe(path_lcm, "wheel_ctrl", parseCmd, &cmd); + + while (true) { + lcm_handle(path_lcm); + } + +err: + close(fd); + return 0; +} + +bool whellInit() { + // 打开串口 + fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); + if (fd < 0) { + fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno)); + return false; + } + + // 设置串口参数 + memset(&tty, 0, sizeof tty); + if (tcgetattr(fd, &tty) != 0) { + fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno)); + return false; + } + + cfsetospeed(&tty, B115200); // 设置输出波特率为115200 + cfsetispeed(&tty, B115200); // 设置输入波特率为115200 + + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars + tty.c_iflag &= ~IGNBRK; // ignore break signal + tty.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 0; // read doesn't block + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl + tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, + // enable reading + tty.c_cflag &= ~(PARENB | PARODD); // shut off parity + tty.c_cflag |= 0; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CRTSCTS; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno)); + return false; + } + return true; +} + +bool wheelSend(byte a, byte a_v, byte b, byte b_v) { + unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}; + byte checksum = 0; + data[2] = a; + data[3] = a_v; + data[4] = b; + data[5] = b_v; + + for (int i = 0; i < 6; i++) { + checksum ^= data[i]; + } + data[6] = checksum; + + // 发送数据 + if (write(fd, data, 7) != 7) { + fprintf(stderr, "Failed to write to the serial port\n"); + return false; + } + + printf("Data %02X %02X %02X %02X %02X %02X %02X wheelSend successfully!\n", + data[0], data[1], data[2], data[3], data[4], data[5], data[6]); + return true; +} + +void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, + const path_ctrl_t *msg, void *userdata) { + byte status = msg->cmd, speed = msg->speed; + switch (status) { + case 1: + wheelSend(0x01, speed, 0x01, speed); + break; + case 2: + wheelSend(0x02, DEFAULT_SPEED, 0x01, DEFAULT_SPEED); + break; + case 3: + wheelSend(0x01, DEFAULT_SPEED, 0x02, DEFAULT_SPEED); + break; + case 0: + default: + wheelSend(0x00, 0x00, 0x00, 0x00); + break; + } +} \ No newline at end of file diff --git a/udp2lcm.c b/udp2lcm.c deleted file mode 100644 index c328a96..0000000 --- a/udp2lcm.c +++ /dev/null @@ -1,198 +0,0 @@ -// Encoded in UTF-8 - -/* - * struct path_ctrl_t - * { - * int8_t cmd; - * int8_t speed; - * } - * 这是与轮控模块通信的数据结构，speed为0-100表示速度 - * cmd为0停，1前，2左，3右 - */ -#include "path/path_ctrl_t.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define MAX_BUFFER_SIZE 1024 -#define PORT 5001 - -pthread_t udpRecv, udpSend; -int8_t heartBeat[9] = {0}; // 用于存储需要发送的心跳信息 -// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改 -// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致 -struct sockaddr_in clientAddr; -char clientIP[20] = ""; -lcm_t *path_ctrl_lcm; - -bool LCMInit(); -void udpSendHandler(); -void udpRecvHandler(); -void parseCmd(const char *buffer, int bytesReceived); - -int main() { - if (!LCMInit()) { - fprintf(stderr, "Error: Failed to initialize LCM\n"); - return -1; - } - // 开启udp接收线程 - pthread_create(&udpRecv, NULL, udpRecvHandler, NULL); - while (true) { - pause(); - } - return 0; -} - -bool LCMInit() { - // TODO: 在这里同意初始化各个LCM变量 - // ATTENTION: - // 是否在这里订阅各个通道、各个通道需要在什么地方订阅，需要慎重考虑 - if ((path_ctrl_lcm = lcm_create(NULL)) == NULL) { - fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n"); - return false; - } - - return true; -} - -void udpRecvHandler() { - int socketfd = -1; - char buffer[MAX_BUFFER_SIZE]; - struct sockaddr_in serverAddr; - socklen_t addrLen = sizeof(serverAddr); - int bytesReceived; - - int retval; - struct pollfd fds; - - printf("udpRecvHandler\n"); - if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { - fprintf(stderr, "Error: Failed to create socket\n"); - return; - } - memset((char *)&serverAddr, 0, sizeof(serverAddr)); - memset(buffer, 0, sizeof(buffer)); - serverAddr.sin_family = AF_INET; - serverAddr.sin_port = htons(PORT); - serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); - - if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) == - -1) { - fprintf(stderr, "Error: Bind failed\n"); - close(socketfd); - return; - } - - bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0, - (struct sockaddr *)&clientAddr, &addrLen); - if (bytesReceived == -1) { - fprintf(stderr, "Error: Failed to receive data\n"); - close(socketfd); - return; - } - printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr), - buffer); - strcpy(clientIP, inet_ntoa(clientAddr.sin_addr)); - - // 在接收到第一条消息之后，开启udp发送线程，用于发送心跳信息 - if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) { - fprintf(stderr, "Error creating udpSend thread\n"); - close(socketfd); - exit(-1); - } - - fds.fd = socketfd; - fds.events = POLLIN; - - while (true) { - retval = poll(&fds, 1, 3000); - if (retval == -1) { - fprintf(stderr, "Error: select failed\n"); - break; - } else if (retval == 0) { - printf("No data within three seconds.\n"); - // out of 3s, stop the wheel - path_ctrl_t path = {0, 0}; - path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path); - } else { - bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0, - (struct sockaddr *)&clientAddr, &addrLen); - if (bytesReceived == -1) { - fprintf(stderr, "Error: Failed to receive data\n"); - continue; - } - printf("Massage from client %s: ", clientIP); - for (int i = 0; i < bytesReceived; i++) { - printf("%02x ", buffer[i]); - } - printf("\n"); - parseCmd(buffer, bytesReceived); - } - } -} - -void udpSendHandler() { - // 创建UDP套接字 - int sockfd = -1; - struct sockaddr_in serverAddr; - if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { - fprintf(stderr, "Error: Failed to create socket\n"); - return; - } - - memset((char *)&serverAddr, 0, sizeof(serverAddr)); - serverAddr.sin_family = AF_INET; // 设置地址族为IPv4 - serverAddr.sin_port = htons(PORT); // 设置端口号 - if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) { - fprintf(stderr, "Error: Failed to convert IP address\n"); - close(sockfd); - return; - } - - while (true) { - sendto(sockfd, (const char *)heartBeat, 9, 0, - (struct sockaddr *)&serverAddr, sizeof(serverAddr)); - sleep(1); - } -} - -void parseCmd(const char *buffer, int bytesReceived) { - // TODO: 完成本函数的内容 - if (buffer == NULL) { - fprintf(stderr, "Error: Invalid message\n"); - return; - } - - if (buffer[0] == 0) { - // 手机端用来与udp2lcm服务器建立连接的初始化消息 - fprintf(stderr, "Why Init cmd here?\n"); - } else if (buffer[0] == 1) { - // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块 - path_ctrl_t path; - path.cmd = buffer[1]; - path.speed = buffer[2]; - path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path); - } else if (buffer[0] == 2) { - /* - * 结束建图，主要有以下几个任务： - * - 向轮控模块发送停止命令 - * - 通知算法模块结束建图（FIXME: 是否接受算法模块的回应？） - * - 拉起http服务器进程 - * - 向手机端通知建图完成 - * - - * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式） - */ - } else if (buffer[0] == 3) { - // 手机发来的终点坐标，转发给算法模块 - } -} \ 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 @@ +cmake_minimum_required(VERSION 3.10) +project(udp2lcm) + +# 设置源文件为本文件夹下的所有.c .cpp文件以及lcmtype文件夹下的所有.c .cpp文件 +aux_source_directory(. DIR_SRCS) +aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS) + +# 生成可执行文件 +add_executable(udp2lcm ${DIR_SRCS}) +target_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 @@ +#include "udp2lcm.h" + +void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime, + int8_t commandid, int8_t robotid, int8_t ndparams, + int8_t niparams, int8_t nsparams, int64_t nbparams) { + if (robotCtrlData == NULL) { + fprintf(stderr, "Error: Invalid robot control data\n"); + return; + } + robotCtrlData->utime = utime; + robotCtrlData->commandid = commandid; + robotCtrlData->robotid = robotid; + robotCtrlData->ndparams = ndparams; + robotCtrlData->dparams = NULL; + robotCtrlData->niparams = niparams; + robotCtrlData->iparams = NULL; + robotCtrlData->nsparams = nsparams; + robotCtrlData->sparams = NULL; + robotCtrlData->nbparams = nbparams; + robotCtrlData->bparams = NULL; + if (ndparams > 0) { + robotCtrlData->dparams = (double *)malloc(ndparams * sizeof(double)); + memset(robotCtrlData->dparams, 0, ndparams * sizeof(double)); + } + if (niparams > 0) { + robotCtrlData->iparams = (int8_t *)malloc(niparams * sizeof(int8_t)); + memset(robotCtrlData->iparams, 0, niparams * sizeof(int8_t)); + } + if (nsparams > 0) { + robotCtrlData->sparams = (char **)malloc(nsparams * sizeof(char *)); + memset(robotCtrlData->sparams, 0, nsparams * sizeof(char *)); + } + if (nbparams > 0) { + robotCtrlData->bparams = (uint8_t *)malloc(nbparams * sizeof(uint8_t)); + memset(robotCtrlData->bparams, 0, nbparams * sizeof(uint8_t)); + } +} + +void freeRobotCtrl(robot_control_t *robotCtrlData) { + if (robotCtrlData == NULL) { + fprintf(stderr, "Error: Invalid robot control data\n"); + return; + } + if (robotCtrlData->dparams != NULL) { + free(robotCtrlData->dparams); + robotCtrlData->dparams = NULL; + } + if (robotCtrlData->iparams != NULL) { + free(robotCtrlData->iparams); + robotCtrlData->iparams = NULL; + } + if (robotCtrlData->sparams != NULL) { + free(robotCtrlData->sparams); + robotCtrlData->sparams = NULL; + } + if (robotCtrlData->bparams != NULL) { + free(robotCtrlData->bparams); + robotCtrlData->bparams = NULL; + } +} \ 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 @@ +#include "udp2lcm.h" + +void udpRecvHandler() { + int socketfd = -1; + char buffer[MAX_BUFFER_SIZE]; + struct sockaddr_in serverAddr; + socklen_t addrLen = sizeof(serverAddr); + int bytesReceived; + + int retval; + struct pollfd fds; + + printf("udpRecvHandler\n"); + if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + fprintf(stderr, "Error: Failed to create socket\n"); + return; + } + memset((char *)&serverAddr, 0, sizeof(serverAddr)); + memset(buffer, 0, sizeof(buffer)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(PORT); + serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) == + -1) { + fprintf(stderr, "Error: Bind failed\n"); + close(socketfd); + return; + } + + bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0, + (struct sockaddr *)&clientAddr, &addrLen); + if (bytesReceived == -1) { + fprintf(stderr, "Error: Failed to receive data\n"); + close(socketfd); + return; + } + printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr), + buffer); + strcpy(clientIP, inet_ntoa(clientAddr.sin_addr)); + + /* + * 接收到来自手机端的第一条消息之后： + * - 启动udpSend线程，用于向手机端发送心跳信息 + * - 通过ROBOT_COMTROL信道向算法模块发送建图开始消息 + * - 7号命令初始坐标、初始角度、雷达扫描最大范围、机器人半径、机器人高 + * - 30号命令开始建图，参数为单个像素点大小 + */ + if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) { + fprintf(stderr, "Error creating udpSend thread\n"); + close(socketfd); + exit(-1); + } + + robot_control_t robotCtrlData; + robotCtrlInit(&robotCtrlData, 0, 7, 0, 7, 1, 0, 0); + robotCtrlData.iparams[0] = 1; + robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); + freeRobotCtrl(&robotCtrlData); + // 随即下达30号命令，开始建图 + robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0); + robotCtrlData.dparams[0] = 0.05; + robotCtrlData.niparams = 1; + robotCtrlData.iparams[0] = 1; + robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); + freeRobotCtrl(&robotCtrlData); + + /* + * 开始接收来自手机端的命令 + * 限定时间为3s，超过3s未收到消息则停止轮子 + * 但不退出循环，防止是网络抖动 + */ + const path_ctrl_t path = {0, 0}; // 就是停止命令，不能改 + fds.fd = socketfd; + fds.events = POLLIN; + while (true) { + retval = poll(&fds, 1, 3000); + if (retval == -1) { + fprintf(stderr, "Error: select failed\n"); + break; + } else if (retval == 0) { + // 超出3s，报错并停止前进 + fprintf(stderr, "No data within three seconds.\n"); + path_ctrl_t_publish(lcm, "wheel_ctrl", &path); + } else { + bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0, + (struct sockaddr *)&clientAddr, &addrLen); + if (bytesReceived == -1) { + fprintf(stderr, "Error: Failed to receive data\n"); + continue; + } + printf("Massage from client %s: ", clientIP); + for (int i = 0; i < bytesReceived; i++) { + printf("%02x ", buffer[i]); + } + printf("\n"); + parseCmd(buffer, bytesReceived); + } + } +} + +void udpSendHandler() { + // 创建UDP套接字 + int sockfd = -1; + struct sockaddr_in serverAddr; + if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + fprintf(stderr, "Error: Failed to create socket\n"); + return; + } + + memset((char *)&serverAddr, 0, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; // 设置地址族为IPv4 + serverAddr.sin_port = htons(PORT); // 设置端口号 + if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) { + fprintf(stderr, "Error: Failed to convert IP address\n"); + close(sockfd); + return; + } + + while (true) { + pthread_mutex_lock(&heartBeatMutex); + sendto(sockfd, (const char *)heartBeat, 9, 0, + (struct sockaddr *)&serverAddr, sizeof(serverAddr)); + pthread_mutex_unlock(&heartBeatMutex); + sleep(1); + } +} \ 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 @@ +// Encoded in UTF-8 + +/* + * struct path_ctrl_t + * { + * int8_t cmd; + * int8_t speed; + * } + * 这是与轮控模块通信的数据结构，speed为0-100表示速度 + * cmd为0停，1前，2左，3右 + */ + +#include "udp2lcm.h" + +pthread_t udpRecv, udpSend; +int8_t heartBeat[9] = {0}; +pthread_mutex_t heartBeatMutex; +struct sockaddr_in clientAddr; +char clientIP[20] = ""; +lcm_t *lcm; + +int main() { + if (!LCMInit()) { + fprintf(stderr, "Error: Failed to initialize LCM\n"); + return -1; + } + // 初始化线程锁 + pthread_mutex_init(&heartBeatMutex, NULL); + // 开启udp接收线程 + pthread_create(&udpRecv, NULL, udpRecvHandler, NULL); + pid_t httpServerPid = -1; + if ((httpServerPid = fork()) == 0) { + // 拉起服务器进程，手机端需要申请的文件/mnt/cf/mapfile/defaultMap.txt + // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改 + chdir("/mnt/cf/mapfile/"); + /* + * execlp使用系统调用exec()执行一个程序 + * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL + * 其他的是传递过去的参数，按一般约定，第一个参数是程序名，不被使用 + * 因而这里出现了两个python + */ + execlp("python", "python", "-m", "http.server", NULL); + } + while (true) { + pause(); + } + return 0; +} + +bool LCMInit() { + if ((lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1")) == NULL) { + fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n"); + return false; + } + return true; +} + +void parseCmd(const char *buffer, int bytesReceived) { + if (buffer == NULL) { + fprintf(stderr, "Error: Invalid message\n"); + return; + } + + path_ctrl_t path; + robot_control_t robotCtrlData; + pose_t curPos; + if (buffer[0] == 0) { + // 手机端用来与udp2lcm服务器建立连接的初始化消息 + // 也是手机端的心跳 + ; + } else if (buffer[0] == 1) { + // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块 + path.cmd = buffer[1]; + path.speed = buffer[2]; + path_ctrl_t_publish(lcm, "wheel_ctrl", &path); + } else if (buffer[0] == 2) { + /* + * 结束建图，主要有以下几个任务： + * - 向轮控模块发送停止命令 + * - 通知算法模块结束建图 + * - 拉起http服务器进程（改由main拉起，省的每次拉起浪费资源） + * - 向手机端通知建图完成 + * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式） + */ + path.cmd = 0; + path.speed = 0; + path_ctrl_t_publish(lcm, "wheel_ctrl", &path); + + robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0); + robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); + + pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL); + } else if (buffer[0] == 3) { + // 手机发来的终点坐标，转发给算法模块 + } +} + +void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel, + const pose_t *msg, void *userdata) { + int16_t x, y, sita; + x = (int16_t)msg->pos[0]; + y = (int16_t)msg->pos[1]; + sita = (int16_t)msg->pos[2]; + while (sita > 180) + sita -= 360; + while (sita < -180) + sita += 360; + pthread_mutex_lock(&heartBeatMutex); + heartBeat[0] = 0x03; + heartBeat[1] = heartBeat[2] = 0; + heartBeat[3] = x >> 8; + heartBeat[4] = x; + heartBeat[5] = y >> 8; + heartBeat[6] = y; + heartBeat[7] = sita >> 8; + heartBeat[8] = sita; + pthread_mutex_unlock(&heartBeatMutex); +} \ 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 @@ +#ifndef UDP2LCM_H +#define UDP2LCM_H + +#include "path_ctrl_t.h" +#include "robot_control_t.h" +#include "pose_t.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAX_BUFFER_SIZE 1024 +#define PORT 5001 + +extern pthread_t udpRecv, udpSend; +extern int8_t heartBeat[9]; // 用于存储需要发送的心跳信息 +extern pthread_mutex_t heartBeatMutex; // 需要给heartBeat加线程锁 +// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改 +// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致 +extern struct sockaddr_in clientAddr; +extern char clientIP[20]; +extern lcm_t *lcm; + +bool LCMInit(); +void udpSendHandler(); +void udpRecvHandler(); +void parseCmd(const char *buffer, int bytesReceived); +void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime, + int8_t commandid, int8_t robotid, int8_t ndparams, + int8_t niparams, int8_t nsparams, int64_t nbparams); +void freeRobotCtrl(robot_control_t *robotCtrlData); +void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel, + const pose_t *msg, void *userdata); + +#endif \ No newline at end of file -- cgit v1.2.3-70-g09d2