From d746146529490663993e489de21d0c15e2d234ad Mon Sep 17 00:00:00 2001 From: We-unite <3205135446@qq.com> Date: Tue, 11 Jun 2024 19:05:45 +0800 Subject: Add udp2lcm server && datastructure to wheel ctrl --- path/path_ctrl.lcm | 5 ++ path/path_ctrl_t.c | 241 +++++++++++++++++++++++++++++++++++++++++++++++++++++ path/path_ctrl_t.h | 143 +++++++++++++++++++++++++++++++ serial.c | 196 +++++++------------------------------------ server.cpp | 167 ------------------------------------- testClient.c | 97 --------------------- udp2lcm.c | 184 ++++++++++++++++++++++++++++++++++++++++ 7 files changed, 602 insertions(+), 431 deletions(-) create mode 100644 path/path_ctrl.lcm create mode 100644 path/path_ctrl_t.c create mode 100644 path/path_ctrl_t.h delete mode 100644 server.cpp delete mode 100644 testClient.c create mode 100644 udp2lcm.c diff --git a/path/path_ctrl.lcm b/path/path_ctrl.lcm new file mode 100644 index 0000000..502e342 --- /dev/null +++ b/path/path_ctrl.lcm @@ -0,0 +1,5 @@ +struct path_ctrl_t +{ + int8_t cmd; + int8_t speed; +} diff --git a/path/path_ctrl_t.c b/path/path_ctrl_t.c new file mode 100644 index 0000000..2298a56 --- /dev/null +++ b/path/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/path/path_ctrl_t.h b/path/path_ctrl_t.h new file mode 100644 index 0000000..3e629eb --- /dev/null +++ b/path/path_ctrl_t.h @@ -0,0 +1,143 @@ +// 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 index 43506ec..6251675 100644 --- a/serial.c +++ b/serial.c @@ -1,50 +1,46 @@ -/* - * 在OpenHarmony系统下开发串口通信的应用与在Linux系统中类似， - * 但需要确保你的设备支持串口操作，并且有适当的驱动支持。 - * - * 以下是一个用C++编写的示例，展示如何在OpenHarmony中打开串口， - * 设置波特率为115200，并发送指定的16进制数。 - * - 打开串口：使用open()函数打开指定的串口设备。 - * - 配置串口：使用termios结构体配置串口参数如波特率、字符大小、奇偶校验等。 - * - 发送数据：使用write()函数将数据写入串口。 - * - 关闭串口：操作完成后，使用close()函数关闭串口。 - * 确保你的OpenHarmony设备有对应的串口设备文件/dev/ttyS1， - * 并且你的应用有足够的权限来访问这个设备。根据你的硬件和操作系统配置，设备文件的路径可能不同。 - */ - -#include +#include "path/path_ctrl_t.h" #include #include -#include -#include -#include #include #include #include #include -#include #include #include -#define FULL_SPEED 1.0f // 全速前进的速度 -#define R 0.15f // 轮距的一半 - typedef unsigned char byte; -int fd; -int port = 5001; +#define MAX_BUFFER_SIZE 1024 + +int fd; // 轮子的串口文件描述符 char portname[50] = "/dev/ttyUSB0"; // 串口设备名 char clientIP[20] = ""; - struct termios tty; -pthread_t udpSend, udpRecv; -const int MAX_BUFFER_SIZE = 1024; -struct sockaddr_in clientAddr; -int whellInit() { +bool whellInit(); +bool wheelSend(byte a, byte a_v, byte b, byte b_v); +void speedControl(byte status, byte speed); +void parseCmd(const path_ctrl_t *msg, void *user); + +int main() { + if (!whellInit()) { + goto err; + } + lcm *path_lcm = lcm_create(NULL); + if (!path_lcm) { + fprintf(stderr, "Failed to create LCM\n"); + goto err; + } + path_ctrl_t_subscribe(path_lcm, "PATH_CTRL", parseCmd); + +err: + close(fd); + return 0; +} + +bool whellInit() { // 打开串口 fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); if (fd < 0) { - // C语言报error fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno)); return false; } @@ -52,7 +48,6 @@ int whellInit() { // 设置串口参数 memset(&tty, 0, sizeof tty); if (tcgetattr(fd, &tty) != 0) { - // cerr << "Error from tcgetattr: " << strerror(errno) << endl; fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno)); return false; } @@ -77,7 +72,6 @@ int whellInit() { tty.c_cflag &= ~CRTSCTS; if (tcsetattr(fd, TCSANOW, &tty) != 0) { - // cerr << "Error from tcsetattr: " << strerror(errno) << endl; fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno)); return false; } @@ -108,7 +102,8 @@ bool wheelSend(byte a, byte a_v, byte b, byte b_v) { return true; } -void speedControl(byte status, byte speed) { +void parseCmd(const path_ctrl_t *msg, void *user) { + byte status = msg->cmd, speed = msg->speed; switch (status) { case 1: wheelSend(0x01, speed, 0x01, speed); @@ -124,137 +119,4 @@ void speedControl(byte status, byte speed) { wheelSend(0x00, 0x00, 0x00, 0x00); break; } -} - -void udpSendThread() { - // 创建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) { - // 每秒向socket发送一次0x30 - byte a = 0x39; - // 发送消息 - sendto(sockfd, (const char *)&a, 1, 0, (struct sockaddr *)&serverAddr, - sizeof(serverAddr)); - printf("Send: Data %02X udpSend successfully!\n", a); - sleep(1); - } -} - -void udpRecvThread() { - int socketfd = -1; - char buffer[MAX_BUFFER_SIZE]; - struct sockaddr_in serverAddr; - socklen_t addrLen = sizeof(serverAddr); - int bytesReceived; - - // fd_set readfds; - // struct timeval tv; - int retval; - struct pollfd fds; - - 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) { - // cerr << "Error: Bind failed" << endl; - 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("Received start message from %s: %s\n", - inet_ntoa(clientAddr.sin_addr), buffer); - strcpy(clientIP, inet_ntoa(clientAddr.sin_addr)); - - if (pthread_create(&udpSend, NULL, udpSendThread, NULL) != 0) { - fprintf(stderr, "Error creating udpSend thread\n"); - close(socketfd); - return; - } - - fds.fd = socketfd; - fds.events = POLLIN; - - while (true) { - // /* Watch sock to see when it has input. */ - // FD_ZERO(&readfds); - // FD_SET(socketfd, &readfds); - // /* Wait up to three seconds. */ - // tv.tv_sec = 3; - // tv.tv_usec = 0; - - // retval = select(socketfd + 1, &readfds, NULL, NULL, &tv); - 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"); - speedControl(0, 0); - // break; - } 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("Received Message from %s: %2x %2x\n", - inet_ntoa(clientAddr.sin_addr), buffer[0], buffer[1]); - speedControl(buffer[0], buffer[1]); - } - } -} - -int main() { - // 关闭1号和2号文件描述符 - // close(1); - // close(2); - // // 打开serial.log文件，并将其复制到1号和2号文件描述符 - // open("serial.log", O_RDWR | O_CREAT, 0666); - // dup(1); - // dup(2); - - if (!whellInit()) { - goto err; - } - - if (pthread_create(&udpRecv, NULL, udpRecvThread, NULL) != 0) { - fprintf(stderr, "Error creating udpRecv thread\n"); - goto err; - } - pthread_join(udpRecv, NULL); - -err: - close(fd); - return 0; -} +} \ No newline at end of file diff --git a/server.cpp b/server.cpp deleted file mode 100644 index 5e41eb8..0000000 --- a/server.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -enum robotState { // 机器人状态枚举 - beforeStart, - pending, - afterEnd, // 建好了图，接下来可选择重新建图或选终点开始路径 - startRoute, -}; - -enum runState { stop, go, left, right }; - -struct ReceiveDataStruct { - int robotId; - robotState state; - int x; - int y; - int r; -}; - -const int PORT = 5001; -const char *SERVER_IP = "0.0.0.0"; -const int MAX_BUFFER_SIZE = 1024; - -struct ReceiveData { - int values[4]; -}; - -bool newDataAvailable = false; -string CLIENT_IP = "127.0.0.1"; -struct sockaddr_in clientAddr; -ReceiveData receivedData = {{0, 1, 2, 3}}; - -// UDP发送线程函数 -void udpSenderThread() { - char values[100] = {0}; - struct sockaddr_in serverAddr; - int sockfd = -1; - - // 创建UDP套接字 - 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; - serverAddr.sin_port = htons(PORT); - if (inet_aton(CLIENT_IP.c_str(), &serverAddr.sin_addr) == 0) { - fprintf(stderr, "Error: Failed to convert IP address\n"); - close(sockfd); - return; - } - - while (true) { - this_thread::sleep_for(chrono::seconds(1)); // 每1秒发送一次 - - // 更新发送的消息 - memset(values, 0, 100); - for (int i = 0; i < 4; ++i) { - // values[i] = receivedData.values[i] + 1; - sprintf(values + strlen(values), "%d ", receivedData.values[i] + 1); - } - sprintf(values + strlen(values), "\n"); - printf("Sending message to %s: %s\n", CLIENT_IP.c_str(), values); - - // 发送消息 - sendto(sockfd, (const char *)values, sizeof(int) * 4, 0, - (struct sockaddr *)&serverAddr, sizeof(serverAddr)); - } - - close(sockfd); -} - -// 主线程接收UDP消息并打印 -void receiveAndPrintMessages() { - int sock; - struct sockaddr_in serverAddr; - socklen_t addrLen = sizeof(clientAddr); - char buffer[MAX_BUFFER_SIZE]; - ReceiveData receivedData; - int pos, bytesReceived; - - // 创建UDP套接字 - if ((sock = 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(sock, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) == - -1) { - cerr << "Error: Bind failed" << endl; - close(sock); - return; - } - - bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0, - (struct sockaddr *)&clientAddr, &addrLen); - if (bytesReceived == -1) { - fprintf(stderr, "Error: Failed to receive data\n"); - return; - } - printf("Received Message from %s: %s\n", inet_ntoa(clientAddr.sin_addr), - buffer); - CLIENT_IP = inet_ntoa(clientAddr.sin_addr); - thread senderThread2(udpSenderThread); - while (true) { - // 接收消息 - bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0, - (struct sockaddr *)&clientAddr, &addrLen); - if (bytesReceived == -1) { - fprintf(stderr, "Error: Failed to receive data\n"); - continue; - } - - printf("Receive %d bytes from %s: \"%s\"\n", bytesReceived, - CLIENT_IP.c_str(), buffer); - pos = 0; - for (int i = 0; i < 4; ++i) { - receivedData.values[i] = atoi(strtok(buffer + pos, " ")); - // pos += log10(receivedData.values[i]) + 2; - // pos需要递增，否则会一直解析第一个数，要求能处理数据为0或负数的情况 - pos += strlen(strtok(buffer + pos, " ")) + 1; - } - newDataAvailable = true; - - // 打印消息 - // CLIENT_IP = inet_ntoa(clientAddr.sin_addr); - // cout << "Received Message from " << CLIENT_IP << ": "; - // printf("Received Message from %s: ", CLIENT_IP.c_str()); - // for (int i = 0; i < 4; ++i) { - // printf("%d ", receivedData.values[i]); - // } - printf("\n"); - } - - close(sock); - senderThread2.join(); -} - -int main() { - thread receiverThread(receiveAndPrintMessages); - receiverThread.join(); - - return 0; -} diff --git a/testClient.c b/testClient.c deleted file mode 100644 index bbd2529..0000000 --- a/testClient.c +++ /dev/null @@ -1,97 +0,0 @@ -// 编写一段C程序，使用pthread创建两个线程 -// 发送线程将数据发送到127.0.0.1的5001端口，每0.5s一次 -// 接收线程接收127.0.0.1的5001端口的数据并打印 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -pthread_t udpSend, udpRecv; -pthread_mutex_t mutex; -int port = 5001; -char clientIP[20] = "127.0.0.1"; - -void udpSenderThread() { - struct sockaddr_in serverAddr; - int sockfd = -1; - - // 创建UDP套接字 - 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; - serverAddr.sin_port = htons(port); - serverAddr.sin_addr.s_addr = inet_addr(clientIP); - - while (1) { - char values[100] = {0}; - // strcpy(values, "Hello, world!"); - pthread_mutex_lock(&mutex); - printf("Input the data you want to send: "); - scanf("%s", values); - pthread_mutex_unlock(&mutex); - sendto(sockfd, values, strlen(values), 0, - (struct sockaddr *)&serverAddr, sizeof(serverAddr)); - // printf("Send: %s\n", values); - sleep(1); - } -} - -void udpReceiverThread() { - int sockfd = -1; - struct sockaddr_in serverAddr; - struct sockaddr_in clientAddr; - socklen_t clientAddrLen = sizeof(clientAddr); - char buffer[1024] = {0}; - - // 创建UDP套接字 - 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; - serverAddr.sin_port = htons(port - 1); - serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); - - if (bind(sockfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) == - -1) { - fprintf(stderr, "Error: Failed to bind socket\n"); - close(sockfd); - return; - } - - while (1) { - pthread_mutex_lock(&mutex); - int len = recvfrom(sockfd, buffer, 1024, 0, - (struct sockaddr *)&clientAddr, &clientAddrLen); - pthread_mutex_unlock(&mutex); - if (len == -1) { - fprintf(stderr, "Error: Failed to receive data\n"); - close(sockfd); - return; - } - buffer[len] = '\0'; - printf("Received: %s\n", buffer); - } -} - -int main() { - pthread_create(&udpSend, NULL, udpSenderThread, NULL); - // pthread_create(&udpRecv, NULL, udpReceiverThread, NULL); - pthread_join(udpSend, NULL); - // pthread_join(udpRecv, NULL); -} \ No newline at end of file diff --git a/udp2lcm.c b/udp2lcm.c new file mode 100644 index 0000000..ec8aee8 --- /dev/null +++ b/udp2lcm.c @@ -0,0 +1,184 @@ +// 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 + +#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); + 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; + + 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"); + speedControl(0, 0); + } 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)); + printf("Send: Data %02x udpSend successfully!\n", a); + sleep(1); + } +} + +void parseCmd(const char *buffer, int bytesReceived) { + // TODO: 完成本函数的内容 + if (buffer == NULL || strlen(buffer) != 9) { + 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 -- cgit v1.2.3-70-g09d2