From fbf334c7dc09fbfb20d4a4a22804a05a0ce5f8b7 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 7 Jul 2024 19:35:44 -0700 Subject: [PATCH 01/34] Add ability to recommend transports from the publisher Start integrating parameters Add static to all message generated functions --- CMakeLists.txt | 1 + CMakeSettings.json | 49 ------------- examples/CMakeLists.txt | 9 +++ include/pubsub/Node.h | 4 +- include/pubsub/Parameter.h | 43 ++++++++++++ include/pubsub/Publisher.h | 1 + include/pubsub/Subscriber.h | 2 +- include/pubsub/TCPTransport.h | 8 ++- include/pubsub_cpp/Node.h | 6 +- msg/Parameters.msg | 5 +- src/Events.c | 79 ++++++++++++++++++--- src/Node.c | 39 +++++++--- src/Parameter.c | 129 ++++++++++++++++++++++++++++++++++ src/Subscriber.c | 5 +- tests/test_pubsub_cpp.cpp | 28 ++++++++ tools/generator.cpp | 14 ++-- tools/pubsub.cpp | 30 +++++++- 17 files changed, 363 insertions(+), 89 deletions(-) delete mode 100644 CMakeSettings.json create mode 100644 include/pubsub/Parameter.h create mode 100644 src/Parameter.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 5854c29..ebd481a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ add_library (pubsub "src/Serialization.c" "src/System.c" "src/UDPTransport.c" + "src/Parameter.c" ) if (UNIX) target_link_libraries(pubsub pubsub_msgs) diff --git a/CMakeSettings.json b/CMakeSettings.json deleted file mode 100644 index 8d94136..0000000 --- a/CMakeSettings.json +++ /dev/null @@ -1,49 +0,0 @@ -{ - // See https://go.microsoft.com//fwlink//?linkid=834763 for more information about this file. - "configurations": [ - { - "name": "x86-Debug", - "generator": "Ninja", - "configurationType": "Debug", - "inheritEnvironments": [ "msvc_x86" ], - "buildRoot": "${projectDir}\\build\\${name}", - "installRoot": "${env.USERPROFILE}\\CMakeBuilds\\${workspaceHash}\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "-v", - "ctestCommandArgs": "" - }, - { - "name": "x86-Release", - "generator": "Ninja", - "configurationType": "RelWithDebInfo", - "inheritEnvironments": [ "msvc_x86" ], - "buildRoot": "${projectDir}\\build\\${name}", - "installRoot": "${env.USERPROFILE}\\CMakeBuilds\\${workspaceHash}\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "-v", - "ctestCommandArgs": "" - }, - { - "name": "x64-Debug", - "generator": "Ninja", - "configurationType": "Debug", - "inheritEnvironments": [ "msvc_x64_x64" ], - "buildRoot": "${projectDir}\\build\\${name}", - "installRoot": "${env.USERPROFILE}\\CMakeBuilds\\${workspaceHash}\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "-v", - "ctestCommandArgs": "" - }, - { - "name": "x64-Release", - "generator": "Ninja", - "configurationType": "RelWithDebInfo", - "inheritEnvironments": [ "msvc_x64_x64" ], - "buildRoot": "${projectDir}\\build\\${name}", - "installRoot": "${env.USERPROFILE}\\CMakeBuilds\\${workspaceHash}\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "-v", - "ctestCommandArgs": "" - } - ] -} \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8c39ba4..38279cf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -15,6 +15,15 @@ add_executable(binding_test "binding_test.c") target_link_libraries(binding_test pubsub_shared pubsub_msgs) add_dependencies(binding_test pubsub_shared pubsub_msgs) +add_executable(light_simulator "light_simulator.cpp") +target_link_libraries(light_simulator pubsub_shared pubsub_msgs) +add_dependencies(light_simulator pubsub_shared pubsub_msgs) + +add_executable(array_members "array_members.cpp") +target_link_libraries(array_members pubsub_cpp pubsub_msgs) +add_dependencies(array_members pubsub_cpp pubsub_msgs) +set_property(TARGET array_members PROPERTY CXX_STANDARD 11) + add_executable(simple_pub_cpp "simple_pub.cpp") target_link_libraries(simple_pub_cpp pubsub_cpp pubsub_msgs) add_dependencies(simple_pub_cpp pubsub_cpp pubsub_msgs) diff --git a/include/pubsub/Node.h b/include/pubsub/Node.h index 06b0bc9..58cd386 100644 --- a/include/pubsub/Node.h +++ b/include/pubsub/Node.h @@ -193,6 +193,8 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched); +void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport); + void ps_node_create_subscriber(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_sub_t* sub, unsigned int queue_size,//make >= 1 @@ -210,7 +212,7 @@ struct ps_subscriber_options unsigned int skip;// skips to every nth message for throttling ps_subscriber_fn_cb_t cb; void* cb_data; - uint32_t preferred_transport;// falls back to udp otherwise + int32_t preferred_transport;// falls back to udp otherwise }; void ps_subscriber_options_init(struct ps_subscriber_options* options); diff --git a/include/pubsub/Parameter.h b/include/pubsub/Parameter.h new file mode 100644 index 0000000..02c4725 --- /dev/null +++ b/include/pubsub/Parameter.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +//#include +#include + +#include +#include +#include +#include + + +#include + +#include + +typedef void(*ps_param_fancy_cb_t)(const char* name, double value, void* data); + +struct ps_parameters +{ + struct pubsub__Parameters msg; + struct ps_pub_t param_pub; + ps_param_fancy_cb_t callback; + void* cb_data; +}; + +void ps_create_parameters(struct ps_node_t* node, struct ps_parameters* params_out, ps_param_fancy_cb_t callback, void* data); + +void ps_destroy_parameters(struct ps_parameters* params); + +void ps_add_parameter_double(struct ps_parameters* params, + const char* name, const char* description, + double value, double min, double max); + +#ifdef __cplusplus +} +#endif diff --git a/include/pubsub/Publisher.h b/include/pubsub/Publisher.h index 638fab5..805e9c9 100644 --- a/include/pubsub/Publisher.h +++ b/include/pubsub/Publisher.h @@ -41,6 +41,7 @@ struct ps_pub_t struct ps_client_t* clients; bool latched;// todo make this an enum of options if we add more + uint8_t recommended_transport; struct ps_msg_t last_message;//only used if latched unsigned int sequence_number; diff --git a/include/pubsub/Subscriber.h b/include/pubsub/Subscriber.h index 9b548b0..5dbd7d5 100644 --- a/include/pubsub/Subscriber.h +++ b/include/pubsub/Subscriber.h @@ -35,7 +35,7 @@ struct ps_sub_t ps_subscriber_fn_cb_t cb; void* cb_data; - unsigned int preferred_transport;// udp or tcp + int preferred_transport;// udp or tcp, or -1 for no preference unsigned int skip; diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 1338b22..83475d2 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -340,6 +340,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no { const int header_size = 5; int len = recv(client->socket, buf, header_size, MSG_PEEK); + //printf("recv %i desired size 0\n", len); if (len == 0) { client->needs_removal = true; @@ -355,6 +356,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no // we actually got the header! start looking for the message len = recv(client->socket, buf, header_size, 0); //connection->packet_type = message_type; + //printf("recv %i from client->socket desired size 0 2\n", len); //client->waiting_for_header = false; client->desired_packet_size = *(uint32_t*)&buf[1]; //printf("Incoming message with %i bytes\n", client->desired_packet_size); @@ -368,6 +370,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no int remaining_size = client->desired_packet_size - client->current_packet_size; // check for new messages and read until we hit packet size int len = recv(client->socket, &client->packet_data[client->current_packet_size], remaining_size, 0); + //printf("recv %i from client->socket\n", len); if (len > 0) { //printf("Read %i bytes of message\n", len); @@ -448,6 +451,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no tv.tv_sec = 0; tv.tv_usec = 0; retval = select(connection->socket + 1, NULL, &wfds, NULL, &tv); + //printf("select\n"); if (retval == -1) { // error? @@ -481,6 +485,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no { const int header_size = 5; int len = recv(connection->socket, buf, header_size, MSG_PEEK); + //printf("len %i\n", len); //printf("peek got: %i\n", len); if (len == 0) { @@ -512,6 +517,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no // check for new messages and read until we hit packet size int len = recv(connection->socket, &connection->packet_data[connection->current_size], remaining_size, 0); + //printf("len %i\n", len); if (len == 0) { // we got disconnected @@ -640,7 +646,7 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub } tclient->queued_messages[0].data = data; tclient->queued_messages[0].length = length + 5; - //printf("dropped message on topic '%s'\n", publisher->topic); + printf("dropped message on topic '%s'\n", publisher->topic); return;// drop it, we are out of queue space } else diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index 257330c..af862d7 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -291,7 +291,7 @@ class Publisher: public PublisherBase public: friend class Subscriber; - Publisher(Node& node, const std::string& topic, bool latched = false)// : topic_(topic) + Publisher(Node& node, const std::string& topic, bool latched = false, int preferred_transport = 0)// : topic_(topic) { node_ = &node; topic_ = topic; @@ -304,7 +304,7 @@ class Publisher: public PublisherBase remapped_topic_ = handle_remap(real_topic, node.getNamespace()); node.lock_.lock(); - ps_node_create_publisher(node.getNode(), remapped_topic_.c_str(), T::GetDefinition(), &publisher_, latched); + ps_node_create_publisher_ex(node.getNode(), remapped_topic_.c_str(), T::GetDefinition(), &publisher_, latched, preferred_transport); node.lock_.unlock(); //add me to the publisher list @@ -545,7 +545,7 @@ class Subscriber: public SubscriberBase public: - Subscriber(Node& node, const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = 0) : cb_(cb), queue_size_(queue_size) + Subscriber(Node& node, const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = -1) : cb_(cb), queue_size_(queue_size) { node_ = &node; diff --git a/msg/Parameters.msg b/msg/Parameters.msg index 991b929..27fdf97 100644 --- a/msg/Parameters.msg +++ b/msg/Parameters.msg @@ -11,9 +11,8 @@ enum uint8 type[] # The current value for the parameter string value[] -# Inclusive maximum valid value for the parameter +# Inclusive minimum valid value for the parameter double min[] -# Inclusive minimum valid value for the parameter -# Not valid for max +# Inclusive maximum valid value for the parameter double max[] diff --git a/src/Events.c b/src/Events.c index cdced01..321876e 100644 --- a/src/Events.c +++ b/src/Events.c @@ -1,6 +1,7 @@ #include #include +#include #ifndef WIN32 #include @@ -14,12 +15,23 @@ void ps_event_set_create(struct ps_event_set_t* set) { #ifdef WIN32 - set->num_handles = 1; + // Old version which had an event per socket + /*set->num_handles = 1; set->handles = (HANDLE*)malloc(sizeof(HANDLE)); set->sockets = (int*)malloc(sizeof(int)); + set->handles[0] = WSACreateEvent(); + set->sockets[0] = -1;*/ + + // New version which uses one event for all sockets + set->num_handles = 3; + set->handles = (HANDLE*)malloc(sizeof(HANDLE)*3); + set->sockets = (int*)malloc(sizeof(int)); + set->handles[0] = WSACreateEvent(); set->sockets[0] = -1; + set->handles[1] = WSACreateEvent(); + set->handles[2] = CreateWaitableTimer(NULL, TRUE, NULL); #else set->fd = epoll_create(1); set->num_events = 0; @@ -53,8 +65,10 @@ void ps_event_set_destroy(struct ps_event_set_t* set) void ps_event_set_add_socket(struct ps_event_set_t* set, int socket) { + //printf("add socket read %i\n", socket); #ifdef WIN32 - // allocate a new spot + // Old version which creates an event per socket + /*// allocate a new spot int cur_size = set->num_handles; HANDLE* new_handles = (HANDLE*)malloc(sizeof(HANDLE)*(cur_size + 1)); int* new_sockets = (int*)malloc(sizeof(int)*(cur_size + 1)); @@ -72,7 +86,10 @@ void ps_event_set_add_socket(struct ps_event_set_t* set, int socket) set->handles[cur_size + 0] = WSACreateEvent(); set->sockets[cur_size + 0] = socket; - WSAEventSelect(socket, set->handles[cur_size + 0], FD_READ); + WSAEventSelect(socket, set->handles[cur_size + 0], FD_READ);*/ + + // New version which always uses the same event + WSAEventSelect(socket, set->handles[1], FD_READ); #else struct epoll_event event; event.events = EPOLLIN; @@ -84,8 +101,10 @@ void ps_event_set_add_socket(struct ps_event_set_t* set, int socket) void ps_event_set_add_socket_write(struct ps_event_set_t* set, int socket) { + //printf("add socket read write %i\n", socket); #ifdef WIN32 - // find the handle and change the select + // Old version which creates an event per socket + /*// find the handle and change the select for (unsigned int i = 0; i < set->num_handles; i++) { if (set->sockets[i] == socket) @@ -93,7 +112,10 @@ void ps_event_set_add_socket_write(struct ps_event_set_t* set, int socket) WSAEventSelect(socket, set->handles[i], FD_READ | FD_WRITE); break; } - } + }*/ + + // New version which always uses the same event + WSAEventSelect(socket, set->handles[1], FD_READ | FD_WRITE); #else struct epoll_event event; event.events = EPOLLIN | EPOLLOUT; @@ -104,7 +126,21 @@ void ps_event_set_add_socket_write(struct ps_event_set_t* set, int socket) void ps_event_set_add_socket_write_only(struct ps_event_set_t* set, int socket) { + //printf("add socket write only %i\n", socket); #ifdef WIN32 + // Old version which creates an event per socket + /*// find the handle and change the select + for (int i = 0; i < set->num_handles; i++) + { + if (set->sockets[i] == socket) + { + WSAEventSelect(socket, set->handles[i], FD_READ | FD_WRITE); + break; + } + }*/ + + // New version which always uses the same event + WSAEventSelect(socket, set->handles[1], FD_WRITE); #else struct epoll_event event; event.events = EPOLLOUT; @@ -115,8 +151,10 @@ void ps_event_set_add_socket_write_only(struct ps_event_set_t* set, int socket) void ps_event_set_remove_socket_write(struct ps_event_set_t* set, int socket) { + //printf("remove socket write %i\n", socket); #ifdef WIN32 - // find the handle and change the select + // Old version which creates an event per socket + /*// find the handle and change the select for (unsigned int i = 0; i < set->num_handles; i++) { if (set->sockets[i] == socket) @@ -124,7 +162,10 @@ void ps_event_set_remove_socket_write(struct ps_event_set_t* set, int socket) WSAEventSelect(socket, set->handles[i], FD_READ); break; } - } + }*/ + + // New version which always uses the same event + WSAEventSelect(socket, set->handles[1], FD_READ); #else struct epoll_event event; event.events = EPOLLIN; @@ -136,8 +177,10 @@ void ps_event_set_remove_socket_write(struct ps_event_set_t* set, int socket) void ps_event_set_remove_socket(struct ps_event_set_t* set, int socket) { + //printf("remove socket %i\n", socket); #ifdef WIN32 - // find the socket to remove then remove it + // Old version which creates an event per socket + /*// find the socket to remove then remove it bool found = false; unsigned int index = 0; for (; index < set->num_handles; index++) @@ -179,8 +222,10 @@ void ps_event_set_remove_socket(struct ps_event_set_t* set, int socket) free(set->handles); free(set->sockets); set->handles = new_handles; - set->sockets = new_sockets; + set->sockets = new_sockets;*/ + // New version which always uses the same event + WSAEventSelect(socket, set->handles[1], 0); #else struct epoll_event event; event.events = EPOLLIN; @@ -206,7 +251,7 @@ void ps_event_set_trigger(struct ps_event_set_t* set) unsigned int ps_event_set_count(const struct ps_event_set_t* set) { #ifdef WIN32 - return set->num_handles-1; + return set->num_handles-1;// this is wrong, but oh well #else return set->num_events; #endif @@ -242,7 +287,19 @@ void ps_event_set_wait(struct ps_event_set_t* set, unsigned int timeout_ms) void ps_event_set_set_timer(struct ps_event_set_t* set, unsigned int timeout_us) { #ifdef WIN32 - // todo + // todo test + // use CreateWaitableTimer and SetWaitableTimer + WaitForMultipleObjectsEx + + LARGE_INTEGER due; + due.QuadPart = 10*timeout_us;// in 100ns intervals + due.QuadPart = -due.QuadPart;// negative = relative + + if (!SetWaitableTimer(set->handles[2], &due, 0, NULL, NULL, 0)) + { + printf("CreateWaitableTimer failed (%d)\n", GetLastError()); + return; + } + #else if (set->timer_fd == 0) { diff --git a/src/Node.c b/src/Node.c index 30af011..199e67e 100644 --- a/src/Node.c +++ b/src/Node.c @@ -114,6 +114,7 @@ void ps_node_advertise(struct ps_pub_t* pub) p->addr = pub->node->addr; p->port = pub->node->port; p->flags = pub->latched ? PS_ADVERTISE_LATCHED : 0; + p->flags |= (pub->recommended_transport << 1) & 0b11111; p->type_hash = pub->message_definition->hash; p->transports = pub->node->supported_transports; p->group_id = pub->node->group_id; @@ -134,8 +135,7 @@ void ps_node_advertise(struct ps_pub_t* pub) int sent_bytes = sendto(pub->node->socket, (const char*)data, off, 0, (struct sockaddr*)&address, sizeof(struct sockaddr_in)); } - -void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched) +void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport) { node->num_pubs++; struct ps_pub_t** old_pubs = node->pubs; @@ -157,10 +157,16 @@ void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const s pub->last_message.data = 0; pub->last_message.len = 0; pub->sequence_number = 0; + pub->recommended_transport = recommended_transport; ps_node_advertise(pub); } +void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched) +{ + ps_node_create_publisher_ex(node, topic, type, pub, latched, 0); +} + // Setup Control-C handlers #ifdef _WIN32 static int ps_shutdown = 0; @@ -523,7 +529,7 @@ void ps_subscriber_options_init(struct ps_subscriber_options* options) options->skip = 0; options->cb = 0; options->cb_data = 0; - options->preferred_transport = PS_TRANSPORT_UDP; + options->preferred_transport = -1;// no preference } void ps_node_create_subscriber_adv(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, @@ -1150,14 +1156,31 @@ int ps_node_spin(struct ps_node_t* node) ep.address = p->addr; ep.port = p->port; + // 0-31 + int recommended_transport = (p->flags >> 1) & 0b11111; + + int preferred_transport = PS_TRANSPORT_UDP; + if (sub->preferred_transport >= 0) + { + preferred_transport = sub->preferred_transport; + } + else + { + preferred_transport = recommended_transport; + } + //printf("preferred transport: %i\n", preferred_transport); + //printf("recommended transport: %i\n", recommended_transport); + + if (preferred_transport != 0) + preferred_transport = (1 << (preferred_transport-1)); // first match udp if its what we want or all that is offered - if (sub->preferred_transport == PS_TRANSPORT_UDP || p->transports == PS_TRANSPORT_UDP) + if (preferred_transport == PS_TRANSPORT_UDP || p->transports == PS_TRANSPORT_UDP) { ps_udp_subscribe(sub, &ep); } else if (node->num_transports == 0) { - printf("ERROR: Transport mismatch. Do not have desired transport.\n"); + printf("ERROR: Transport mismatch on topic '%s'. Do not have desired transport %i.\n", topic, preferred_transport); } else { @@ -1166,14 +1189,14 @@ int ps_node_spin(struct ps_node_t* node) for (int i = 0; i < node->num_transports; i++) { struct ps_transport_t* transport = &node->transports[i]; - if ((transport->uuid & sub->preferred_transport) != 0) + if ((transport->uuid & preferred_transport) != 0) { int data_index = 0; for (int i = 0; i < 16; i++) { if ((p->transports & (1 << i)) != 0) { - if (sub->preferred_transport == (1 << i)) + if (preferred_transport == (1 << i)) { // this is it break; @@ -1191,6 +1214,7 @@ int ps_node_spin(struct ps_node_t* node) if (!found) { // Otherwise fallback to udp + //printf("Match not found, falling back to udp\n"); ps_udp_subscribe(sub, &ep); } } @@ -1374,7 +1398,6 @@ void ps_node_set_parameter(struct ps_node_t* node, const char* name, double valu data[0] = PS_UDP_PROTOCOL_PARAM_CHANGE; *(double*)&data[1] = value; - int off = sizeof(struct ps_advertise_req_t); int len = serialize_string(&data[1+8], name) + 9; //also add other info... diff --git a/src/Parameter.c b/src/Parameter.c new file mode 100644 index 0000000..55ef4ce --- /dev/null +++ b/src/Parameter.c @@ -0,0 +1,129 @@ +#include + +//#include +#include + +#include +#include +#include +#include + +//#include + +#include + +static double param_internal_callback(const char* name, double value, void* data) +{ + struct ps_parameters* params = (struct ps_parameters*)data; + for (int i = 0; i < params->msg.name_length; i++) + { + if (strcmp(name, params->msg.name[i]) == 0) + { + // enforce min/max + if (value < params->msg.min[i]) + { + value = params->msg.min[i]; + } + else if (value > params->msg.max[i]) + { + value = params->msg.max[i]; + } + free(params->msg.value[i]); + char valstr[50]; + sprintf(valstr, "%lf", value); + params->msg.value[i] = strdup(valstr); + params->callback(name, value, params->cb_data); + ps_pub_publish_ez(¶ms->param_pub, ¶ms->msg); + return value; + } + } + printf("could not find parameter %s\n", name); + + return nan(""); +} + +void ps_create_parameters(struct ps_node_t* node, struct ps_parameters* params_out, ps_param_fancy_cb_t callback, void* data) +{ + node->param_cb = param_internal_callback; + node->param_cb_data = (void*)params_out; + params_out->callback = callback; + params_out->cb_data = data; + params_out->msg.name_length = 0; + ps_node_create_publisher(node, "/parameters", &pubsub__Parameters_def, ¶ms_out->param_pub, true); +} + +void ps_destroy_parameters(struct ps_parameters* params) +{ + params->param_pub.node->param_cb = 0; + if (params->msg.name_length > 0) + { + // free everything! + for (int i = 0; i < params->msg.name_length; i++) + { + free(params->msg.name[i]); + free(params->msg.value[i]); + free(params->msg.description[i]); + } + free(params->msg.name); + free(params->msg.value); + free(params->msg.description); + free(params->msg.type); + free(params->msg.min); + free(params->msg.max); + } + ps_pub_destroy(¶ms->param_pub); +} +/* { FT_String, FF_NONE, "name", 0, 0 }, + { FT_String, FF_NONE, "description", 0, 0 }, + { FT_UInt8, FF_ENUM, "type", 0, 0 }, + { FT_String, FF_NONE, "value", 0, 0 }, + { FT_Float64, FF_NONE, "min", 0, 0 }, + { FT_Float64, FF_NONE, "max", 0, 0 }, */ +void ps_add_parameter_double(struct ps_parameters* params, + const char* name, const char* description, + double value, double min, double max) +{ + int old_len = params->msg.name_length; + int new_len = ++params->msg.name_length; + params->msg.name_length = params->msg.value_length = params->msg.min_length = + params->msg.max_length = params->msg.description_length = params->msg.type_length = new_len; + char** newname = (char**)malloc(sizeof(char*)*new_len); + char** newdesc = (char**)malloc(sizeof(char*)*new_len); + char** newvalue = (char**)malloc(sizeof(char*)*new_len); + double* newmin = (double*)malloc(sizeof(double)*new_len); + double* newmax = (double*)malloc(sizeof(double)*new_len); + uint8_t* newtype = (uint8_t*)malloc(sizeof(uint8_t)*new_len); + for (int i = 0; i < old_len; i++) + { + newname[i] = params->msg.name[i]; + newdesc[i] = params->msg.description[i]; + newvalue[i] = params->msg.value[i]; + newtype[i] = params->msg.type[i]; + newmin[i] = params->msg.min[i]; + newmax[i] = params->msg.max[i]; + } + newname[old_len] = strdup(name); + newdesc[old_len] = strdup(description); + char valstr[50]; + sprintf(valstr, "%lf", value); + newvalue[old_len] = strdup(valstr); + newtype[old_len] = PARAMETERS_DOUBLE; + newmin[old_len] = min; + newmax[old_len] = max; + if (old_len != 0) + { + free(params->msg.name); + free(params->msg.value); + free(params->msg.description); + free(params->msg.type); + free(params->msg.min); + free(params->msg.max); + } + params->msg.name = newname; + params->msg.description = newdesc; + params->msg.value = newvalue; + params->msg.type = newtype; + params->msg.min = newmin; + params->msg.max = newmax; + ps_pub_publish_ez(¶ms->param_pub, ¶ms->msg); +} diff --git a/src/Subscriber.c b/src/Subscriber.c index 19119f1..cc0f24d 100644 --- a/src/Subscriber.c +++ b/src/Subscriber.c @@ -28,7 +28,8 @@ void ps_sub_enqueue(struct ps_sub_t* sub, void* data, int data_size, const struc else if (sub->queue_size == sub->queue_len) { // we'll replace the item at the back by shifting the queue around - free(sub->queue[sub->queue_start]); +//okay, we can try and keep the message as serialized then only deserialize on deque + free(sub->queue[sub->queue_start]);// hmm, this is a memory leak for complex types.... // add at the front sub->queue[new_start] = data; sub->queue_start = new_start; @@ -84,7 +85,7 @@ void ps_sub_destroy(struct ps_sub_t* sub) { int index = (sub->queue_start + i)%sub->queue_size; if (sub->queue[index] != 0) - free(sub->queue[index]); + free(sub->queue[index]);// memory leak for complex types } free(sub->queue); } diff --git a/tests/test_pubsub_cpp.cpp b/tests/test_pubsub_cpp.cpp index 8967804..95f0703 100644 --- a/tests/test_pubsub_cpp.cpp +++ b/tests/test_pubsub_cpp.cpp @@ -45,6 +45,34 @@ TEST(test_publisher_subscriber_close_cpp, []() { sub.close(); sub.close(); }); + +/*TEST(test_publish_subscribe_latched_cpp, []() { + // test that latched topics make it through the local message passing between nodes + pubsub::Node node("simple_publisher"); + + pubsub::Publisher string_pub(node, "/data", true); + + pubsub::msg::String omsg; + omsg.value = "Hello"; + string_pub.publish(omsg); + + + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node); + + bool got_message = false; + pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { + printf("Got message %s in sub1\n", msg->value); + EXPECT(strcmp(omsg.value, msg->value) == 0); + got_message = true; + spinner.stop(); + }, 10); + + spinner.wait(); + EXPECT(got_message); +});*/ + + TEST(test_publish_subscribe_cpp, []() { // test that normal messages make it through message passing pubsub::Node node("simple_publisher"); diff --git a/tools/generator.cpp b/tools/generator.cpp index 7484e63..cbe8a86 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -462,7 +462,7 @@ std::string generate(const char* definition, const char* name) //ps_message_definition_t std_msgs_String_def = { 123456789, "std_msgs/String", 1, std_msgs_String_fields }; // generate the fields - output += "struct ps_msg_field_t " + type_name + "_fields[] = {\n"; + output += "static struct ps_msg_field_t " + type_name + "_fields[] = {\n"; for (auto& field : fields) { if (field.getTypeEnum() == "FT_Struct") @@ -490,7 +490,7 @@ std::string generate(const char* definition, const char* name) // generate enum metadata if (enumerations.size()) { - output += "struct ps_msg_enum_t " + type_name + "_enums[] = {\n"; + output += "static struct ps_msg_enum_t " + type_name + "_enums[] = {\n"; for (auto& e: enumerations) { output += " {\"" + e.name + "\", " + e.value + ", " + std::to_string(e.field_num) + "},\n"; @@ -536,14 +536,14 @@ std::string generate(const char* definition, const char* name) if (is_pure) { //generate simple de/serializaton - output += "void* " + type_name + "_decode(const void* data, struct ps_allocator_t* allocator)\n{\n"; + output += "static void* " + type_name + "_decode(const void* data, struct ps_allocator_t* allocator)\n{\n"; output += " struct " + type_name + "* out = (struct " + type_name + "*)allocator->alloc(sizeof(struct " + type_name + "), allocator->context);\n"; output += " *out = *(struct " + type_name + "*)data;\n"; output += " return out;\n"; output += "}\n\n"; // now for encode - output += "struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* msg)\n{\n"; + output += "static struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* msg)\n{\n"; output += " int len = sizeof(struct " + type_name + ");\n"; output += " struct ps_msg_t omsg;\n"; output += " ps_msg_alloc(len, allocator, &omsg);\n"; @@ -553,7 +553,7 @@ std::string generate(const char* definition, const char* name) else { //need to split it in sections between the strings - output += "void* " + type_name + "_decode(const void* data, struct ps_allocator_t* allocator)\n{\n"; + output += "static void* " + type_name + "_decode(const void* data, struct ps_allocator_t* allocator)\n{\n"; output += " char* p = (char*)data;\n"; output += " int len = sizeof(struct "+type_name+");\n"; output += " struct "+type_name+"* out = (struct " + type_name + "*)allocator->alloc(len, allocator->context);\n"; @@ -627,7 +627,7 @@ std::string generate(const char* definition, const char* name) output += "}\n\n"; //typedef ps_msg_t(*ps_fn_encode_t)(ps_allocator_t* allocator, const void* msg); - output += "struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* data)\n{\n"; + output += "static struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* data)\n{\n"; output += " const struct " + type_name + "* msg = (const struct " + type_name + "*)data;\n"; output += " int len = sizeof(struct " + type_name + ");\n"; output += " // calculate the encoded length of the message\n"; @@ -737,7 +737,7 @@ std::string generate(const char* definition, const char* name) { field_count += f.type->fields.size(); } - output += "struct ps_message_definition_t " + type_name + "_def = { "; + output += "static struct ps_message_definition_t " + type_name + "_def = { "; if (enumerations.size() == 0) { output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, 0, 0 };\n"; diff --git a/tools/pubsub.cpp b/tools/pubsub.cpp index 26a4ef4..56f24f7 100644 --- a/tools/pubsub.cpp +++ b/tools/pubsub.cpp @@ -124,6 +124,13 @@ int topic_info(int num_args, char** args, ps_node_t* node) std::cout << "Type: " << info->second.type << "\n"; std::cout << "Latched: " << (((info->second.flags & PS_ADVERTISE_LATCHED) != 0) ? "True\n" : "False\n"); + int recommended_transport = ((info->second.flags & 0b111110) >> 1); + std::string transport = "UNKNOWN (" + std::to_string(recommended_transport) + ")"; + if (recommended_transport == 0) + transport = "UDP"; + else if (recommended_transport == 1) + transport = "TCP"; + std::cout << "Recommended Transport: " << transport << "\n"; std::cout << "Published by:\n"; for (auto pub : info->second.publishers) { @@ -211,6 +218,7 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) parser.AddOption({ "n" }, "Number of messages to echo.", "0"); parser.AddOption({ "skip", "s" }, "Skip factor for the subscriber.", "0"); parser.AddFlag({ "tcp" }, "Prefer the TCP transport."); + parser.AddFlag({ "udp" }, "Prefer the UDP transport."); parser.AddFlag({ "no-arr" }, "Don't print out the contents of arrays in messages."); parser.AddOption({ "f", "field" }, "Print out just the value of a specific field."); @@ -223,6 +231,12 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) return 0; } + if (parser.GetBool("tcp") && parser.GetBool("udp")) + { + printf("ERROR: Cannot provide both --tcp and --udp options.\n"); + exit(2); + } + static bool print_info = parser.GetBool("i"); double vn = parser.GetDouble("n"); if (vn <= 0) @@ -231,7 +245,6 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) } static unsigned long long int n = vn; int skip = parser.GetDouble("s"); - bool tcp = parser.GetBool("tcp"); static bool no_arr = parser.GetBool("no-arr"); @@ -310,7 +323,9 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) options.queue_size = 0; options.allocator = 0; options.ignore_local = false; - options.preferred_transport = tcp ? 1 : 0; + options.preferred_transport = -1; + options.preferred_transport = parser.GetBool("tcp") ? 1 : options.preferred_transport; + options.preferred_transport = parser.GetBool("udp") ? 0 : options.preferred_transport; options.cb = [](void* message, unsigned int size, void* data, const ps_msg_info_t* info) { // get and deserialize the messages @@ -826,12 +841,19 @@ int main(int num_args_real, char** args) pubsub::ArgParser parser; parser.AddOption({ "w", "window" }, "Window size for averaging.", "100"); parser.AddFlag({ "tcp" }, "Prefer the TCP transport."); + parser.AddFlag({ "udp" }, "Prefer the UDP transport."); if (subverb == "hz") parser.SetUsage("Usage: info topic hz TOPIC\n\nDetermines the rate of publication for a given topic."); else parser.SetUsage("Usage: info topic bw TOPIC\n\nDetermines the single subscriber bandwidth for a given topic."); parser.Parse(args, num_args, 2); + if (parser.GetBool("tcp") && parser.GetBool("udp")) + { + printf("ERROR: Cannot provide both --tcp and --udp options.\n"); + exit(2); + } + // create a subscriber ps_sub_t sub; std::vector todo_msgs; @@ -869,7 +891,9 @@ int main(int num_args_real, char** args) ps_subscriber_options_init(&opts); opts.cb = cb; opts.cb_data = &message_times; - opts.preferred_transport = parser.GetBool("tcp") ? 1 : 0; + opts.preferred_transport = -1; + opts.preferred_transport = parser.GetBool("tcp") ? 1 : opts.preferred_transport; + opts.preferred_transport = parser.GetBool("udp") ? 0 : opts.preferred_transport; ps_node_create_subscriber_adv(&node, info->first.c_str(), 0, &sub, &opts); break; } From 91c8cdc5f384d417e7190108edcc06250930cbf7 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 7 Jul 2024 20:58:08 -0700 Subject: [PATCH 02/34] Add free to msg_def struct to fix memory leak and make it easier to free messages in C --- include/pubsub/Serialization.h | 4 ++- src/Node.c | 2 +- src/Serialization.c | 4 +++ src/Subscriber.c | 61 +++++++++++++++++++++------------- tools/generator.cpp | 47 ++++++++++++++++++++++++-- 5 files changed, 90 insertions(+), 28 deletions(-) diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index c27850c..dfae52c 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -11,7 +11,7 @@ extern "C" struct ps_allocator_t { void*(*alloc)(unsigned int size, void* context); - void(*free)(void*); + void(*free)(void*, void* context); void* context; }; @@ -71,6 +71,7 @@ extern "C" struct ps_allocator_t; typedef struct ps_msg_t(*ps_fn_encode_t)(struct ps_allocator_t* allocator, const void* msg); typedef void*(*ps_fn_decode_t)(const void* data, struct ps_allocator_t* allocator);// allocates the message + typedef void (*ps_fn_free_t)(struct ps_allocator_t* allocator, void* msg);// frees the message struct ps_message_definition_t { unsigned int hash; @@ -79,6 +80,7 @@ extern "C" struct ps_msg_field_t* fields; ps_fn_encode_t encode; ps_fn_decode_t decode; + ps_fn_free_t free; unsigned int num_enums; struct ps_msg_enum_t* enums; }; diff --git a/src/Node.c b/src/Node.c index 199e67e..86e57a7 100644 --- a/src/Node.c +++ b/src/Node.c @@ -514,7 +514,7 @@ void* ps_malloc_alloc(unsigned int size, void* _) return malloc(size); } -void ps_malloc_free(void* data) +void ps_malloc_free(void* data, void* _) { free(data); } diff --git a/src/Serialization.c b/src/Serialization.c index 3b604fe..814fb6b 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -78,6 +78,9 @@ int ps_serialize_message_definition(void* start, const struct ps_message_definit void ps_copy_message_definition(struct ps_message_definition_t* dst, const struct ps_message_definition_t* src) { + dst->encode = src->encode; + dst->decode = src->decode; + dst->free = src->free; dst->num_fields = src->num_fields; dst->hash = src->hash; dst->fields = (struct ps_msg_field_t*)malloc(sizeof(struct ps_msg_field_t)*dst->num_fields); @@ -115,6 +118,7 @@ void ps_deserialize_message_definition(const void * start, struct ps_message_def definition->num_enums = hdr->num_enums; definition->decode = 0; definition->encode = 0; + definition->free = 0; definition->name = 0; definition->fields = (struct ps_msg_field_t*)malloc(sizeof(struct ps_msg_field_t)*definition->num_fields); diff --git a/src/Subscriber.c b/src/Subscriber.c index cc0f24d..3ea0c4d 100644 --- a/src/Subscriber.c +++ b/src/Subscriber.c @@ -28,8 +28,14 @@ void ps_sub_enqueue(struct ps_sub_t* sub, void* data, int data_size, const struc else if (sub->queue_size == sub->queue_len) { // we'll replace the item at the back by shifting the queue around -//okay, we can try and keep the message as serialized then only deserialize on deque - free(sub->queue[sub->queue_start]);// hmm, this is a memory leak for complex types.... + if (sub->type) + { + sub->type->free(sub->allocator, sub->queue[sub->queue_start]); + } + else + { + free(sub->queue[sub->queue_start]); + } // add at the front sub->queue[new_start] = data; sub->queue_start = new_start; @@ -56,28 +62,28 @@ void ps_sub_destroy(struct ps_sub_t* sub) //remove it from my list of subs sub->node->num_subs--; - if (sub->node->num_subs == 0) - { - free(sub->node->subs); - sub->node->subs = 0; - } - else - { - struct ps_sub_t** old_subs = sub->node->subs; - sub->node->subs = (struct ps_sub_t**)malloc(sizeof(struct ps_sub_t*)*sub->node->num_subs); - int ind = 0; - for (unsigned int i = 0; i < sub->node->num_subs+1; i++) - { - if (old_subs[i] == sub) - { - //skip me - } - else + if (sub->node->num_subs == 0) + { + free(sub->node->subs); + sub->node->subs = 0; + } + else + { + struct ps_sub_t** old_subs = sub->node->subs; + sub->node->subs = (struct ps_sub_t**)malloc(sizeof(struct ps_sub_t*)*sub->node->num_subs); + int ind = 0; + for (unsigned int i = 0; i < sub->node->num_subs+1; i++) { - sub->node->subs[ind++] = old_subs[i]; + if (old_subs[i] == sub) + { + //skip me + } + else + { + sub->node->subs[ind++] = old_subs[i]; + } } - } - free(old_subs); + free(old_subs); } // free any queued up received messages and the queue itself @@ -85,7 +91,16 @@ void ps_sub_destroy(struct ps_sub_t* sub) { int index = (sub->queue_start + i)%sub->queue_size; if (sub->queue[index] != 0) - free(sub->queue[index]);// memory leak for complex types + { + if (sub->type) + { + sub->type->free(sub->allocator, sub->queue[index]); + } + else + { + free(sub->queue[index]); + } + } } free(sub->queue); } diff --git a/tools/generator.cpp b/tools/generator.cpp index cbe8a86..a314f01 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -548,7 +548,12 @@ std::string generate(const char* definition, const char* name) output += " struct ps_msg_t omsg;\n"; output += " ps_msg_alloc(len, allocator, &omsg);\n"; output += " memcpy(ps_get_msg_start(omsg.data), msg, len);\n"; - output += " return omsg;\n}\n"; + output += " return omsg;\n}\n\n"; + + // finally free todo use allocator + output += "static void " + type_name + "_free(struct ps_allocator_t* allocator, void* msg)\n{\n"; + output += " allocator->free(msg, allocator->context);\n"; + output += "}\n\n"; } else { @@ -729,6 +734,42 @@ std::string generate(const char* definition, const char* name) } output += " return omsg;\n"; output += "}\n"; + + // finally free + output += "static void " + type_name + "_free(struct ps_allocator_t* allocator, void* data)\n{\n"; + output += " struct " + type_name + "* msg = (struct " + type_name + "*)data;\n"; + for (size_t i = 0; i < fields.size(); i++) + { + if (fields[i].type == string_type) + { + if (fields[i].array_size == 1) + { + output += " allocator->free(msg->" + fields[i].name + ", allocator->context);\n"; + } + else + { + if (fields[i].array_size == 0) + { + output += " int num_" + fields[i].name + " = msg->" + fields[i].name + "_length;\n"; + } + else + { + output += " int num_" + fields[i].name + " = " + std::to_string(fields[i].array_size) + ";\n"; + } + + output += " for (int i = 0; i < num_" + fields[i].name + "; i++) {\n"; + output += " allocator->free(msg->" + fields[i].name + "[i], allocator->context);\n"; + output += " }\n"; + output += " allocator->free(msg->" + fields[i].name + ", allocator->context);\n"; + } + } + else if (fields[i].array_size == 0) + { + output += " allocator->free(msg->" + fields[i].name + ", allocator->context);\n"; + } + } + output += " allocator->free(msg, allocator->context);\n"; + output += "}\n\n"; } // generate the actual message definition @@ -740,11 +781,11 @@ std::string generate(const char* definition, const char* name) output += "static struct ps_message_definition_t " + type_name + "_def = { "; if (enumerations.size() == 0) { - output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, 0, 0 };\n"; + output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, 0, 0 };\n"; } else { - output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + std::to_string(enumerations.size()) + ", " + type_name + "_enums };\n"; + output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, " + std::to_string(enumerations.size()) + ", " + type_name + "_enums };\n"; } output += "\n#ifdef __cplusplus\n"; From 34266c2ed287779a882f1e130e0ae47abc88761c Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 17 Sep 2024 21:26:52 -0700 Subject: [PATCH 03/34] Fix some missing files --- examples/CMakeLists.txt | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 38279cf..951415d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -15,15 +15,6 @@ add_executable(binding_test "binding_test.c") target_link_libraries(binding_test pubsub_shared pubsub_msgs) add_dependencies(binding_test pubsub_shared pubsub_msgs) -add_executable(light_simulator "light_simulator.cpp") -target_link_libraries(light_simulator pubsub_shared pubsub_msgs) -add_dependencies(light_simulator pubsub_shared pubsub_msgs) - -add_executable(array_members "array_members.cpp") -target_link_libraries(array_members pubsub_cpp pubsub_msgs) -add_dependencies(array_members pubsub_cpp pubsub_msgs) -set_property(TARGET array_members PROPERTY CXX_STANDARD 11) - add_executable(simple_pub_cpp "simple_pub.cpp") target_link_libraries(simple_pub_cpp pubsub_cpp pubsub_msgs) add_dependencies(simple_pub_cpp pubsub_cpp pubsub_msgs) @@ -42,9 +33,3 @@ set_property(TARGET simple_sub_cpp PROPERTY CXX_STANDARD 11) add_executable(read_param "read_param.c") target_link_libraries(read_param pubsub pubsub_msgs) add_dependencies(read_param pubsub pubsub_msgs) - -#install(TARGETS pubsubtest -# ARCHIVE DESTINATION "s" -# LIBRARY DESTINATION "s" -# RUNTIME DESTINATION "s" -#) From f2dc92ad50969d005890a89c82eb1bbc85f8f3d3 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 17 Sep 2024 21:28:23 -0700 Subject: [PATCH 04/34] Run tests on linux --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 85fb0af..6648aca 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -20,5 +20,5 @@ jobs: run: cmake . - name: cmake build run: cmake --build . -# - name: Run Tests -# run: cd tests && ctest --output-on-failure + - name: Run Tests + run: cd tests && ctest --output-on-failure From 214fe6ad808d70be1576d82e74b15604a3b6a922 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 17 Sep 2024 21:31:56 -0700 Subject: [PATCH 05/34] Fix random include breaking things --- .github/workflows/ccpp.yml | 2 +- include/pubsub/Parameter.h | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6648aca..c534f33 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,4 +21,4 @@ jobs: - name: cmake build run: cmake --build . - name: Run Tests - run: cd tests && ctest --output-on-failure + run: cd tests && ctest --output-on-failure -VV diff --git a/include/pubsub/Parameter.h b/include/pubsub/Parameter.h index 02c4725..055abcf 100644 --- a/include/pubsub/Parameter.h +++ b/include/pubsub/Parameter.h @@ -1,7 +1,5 @@ #pragma once -#include - #ifdef __cplusplus extern "C" { @@ -15,7 +13,6 @@ extern "C" #include #include - #include #include From e6ccaaa6310ce0b7f13d3e97cd155d82d707a139 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 17 Sep 2024 21:34:35 -0700 Subject: [PATCH 06/34] Add timeout for tests --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c534f33..7b320c8 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,4 +21,4 @@ jobs: - name: cmake build run: cmake --build . - name: Run Tests - run: cd tests && ctest --output-on-failure -VV + run: cd tests && ctest -VV --timeout 5 From aef549cdfdd10878dafbd4f70f21a28c5d69905a Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 17 Sep 2024 21:38:58 -0700 Subject: [PATCH 07/34] Fix missing dependency on windows Disable tests again until I can get them working in CI --- .github/workflows/ccpp.yml | 4 ++-- CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 7b320c8..0aa62b2 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -20,5 +20,5 @@ jobs: run: cmake . - name: cmake build run: cmake --build . - - name: Run Tests - run: cd tests && ctest -VV --timeout 5 +# - name: Run Tests +# run: cd tests && ctest -VV --timeout 5 diff --git a/CMakeLists.txt b/CMakeLists.txt index ebd481a..e9d89de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ add_library (pubsub if (UNIX) target_link_libraries(pubsub pubsub_msgs) else() -target_link_libraries(pubsub winmm) +target_link_libraries(pubsub winmm pubsub_msgs) endif() set_target_properties(pubsub PROPERTIES DEBUG_POSTFIX "d") target_include_directories(pubsub PUBLIC include/) From 893b6d909465f0bd7a18efbd3ab31936e5287a53 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 27 Oct 2024 13:02:57 -0700 Subject: [PATCH 08/34] Avoid some copies in TCP transport and reduce number of send calls Note, breaks TCP protocol compatibility Fix a memory leak --- include/pubsub/Node.h | 6 +- include/pubsub/Publisher.h | 2 +- include/pubsub/Serialization.h | 11 ++- include/pubsub/TCPTransport.h | 147 ++++++++++++--------------------- include/pubsub/UDPTransport.h | 3 +- src/Node.c | 3 +- src/Publisher.c | 82 ++++++++++++------ src/Serialization.c | 31 +++++-- src/UDPTransport.c | 5 +- 9 files changed, 151 insertions(+), 139 deletions(-) diff --git a/include/pubsub/Node.h b/include/pubsub/Node.h index 58cd386..228582f 100644 --- a/include/pubsub/Node.h +++ b/include/pubsub/Node.h @@ -29,7 +29,8 @@ struct ps_endpoint_t; struct ps_client_t; struct ps_subscribe_req_t; struct ps_allocator_t; -typedef void(*ps_transport_fn_pub_t)(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, const void* message, uint32_t length); +struct ps_msg_ref_t; +typedef void(*ps_transport_fn_pub_t)(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, struct ps_msg_ref_t* message); typedef int(*ps_transport_fn_spin_t)(struct ps_transport_t* transport, struct ps_node_t* node); typedef void(*ps_transport_fn_add_publisher_t)(struct ps_transport_t* transport, struct ps_pub_t* publisher); typedef void(*ps_transport_fn_remove_publisher_t)(struct ps_transport_t* transport, struct ps_pub_t* publisher); @@ -129,10 +130,9 @@ struct ps_msg_info_t struct ps_msg_header { uint8_t pid;//packet type id + uint32_t length;//message length uint32_t id;//stream id uint16_t seq;//sequence number - uint8_t index; - uint8_t count; }; #pragma pack(pop) diff --git a/include/pubsub/Publisher.h b/include/pubsub/Publisher.h index 805e9c9..8bc7807 100644 --- a/include/pubsub/Publisher.h +++ b/include/pubsub/Publisher.h @@ -43,7 +43,7 @@ struct ps_pub_t bool latched;// todo make this an enum of options if we add more uint8_t recommended_transport; - struct ps_msg_t last_message;//only used if latched + struct ps_msg_ref_t* last_message;//only used if latched unsigned int sequence_number; }; diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index dfae52c..80b43a5 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -60,13 +60,22 @@ extern "C" int field;// the field this is associated with in the message }; - // encoded message struct ps_msg_t { void* data; unsigned int len; }; + + struct ps_msg_ref_t + { + void* data; + unsigned int len; + unsigned int refcount; + }; + + void ps_msg_ref_add(struct ps_msg_ref_t* msg); + void ps_msg_ref_free(struct ps_msg_ref_t* msg); struct ps_allocator_t; typedef struct ps_msg_t(*ps_fn_encode_t)(struct ps_allocator_t* allocator, const void* msg); diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 83475d2..675dc5b 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -8,6 +8,7 @@ #include #include #include +#include //#include #include @@ -20,6 +21,12 @@ #define PUBSUB_TCP_TRANSPORT 1 +enum +{ + PS_TCP_PROTOCOL_DATA = PS_UDP_PROTOCOL_DATA, + PS_TCP_PROTOCOL_MESSAGE_DEFINITION = 0x03, +}; + /* typedef void(*ps_transport_fn_pub_t)(struct ps_transport_t* transport, struct ps_pub_t* publisher, void* message); @@ -62,8 +69,7 @@ struct ps_tcp_transport_connection struct ps_tcp_client_queued_message_t { - char* data; - int32_t length; + struct ps_msg_ref_t* msg; }; struct ps_tcp_client_t @@ -77,7 +83,7 @@ struct ps_tcp_client_t int32_t desired_packet_size; char* packet_data; - char* queued_message; + struct ps_msg_ref_t* queued_message; int32_t queued_message_length; int32_t queued_message_written; @@ -124,7 +130,7 @@ void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, s if (transport->clients[i].queued_message) { - free(transport->clients[i].queued_message); + ps_msg_ref_free(transport->clients[i].queued_message); } // free queued messages @@ -132,7 +138,7 @@ void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, s { for (int j = 0; j < transport->clients[i].num_queued_messages; j++) { - free(transport->clients[i].queued_messages[j].data); + ps_msg_ref_free(transport->clients[i].queued_messages[j].msg); } free(transport->clients[i].queued_messages); } @@ -292,16 +298,16 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no if (client->queued_message_written == client->queued_message_length) { //printf("Message sent.\n"); - free(client->queued_message); + ps_msg_ref_free(client->queued_message); client->queued_message = 0; // we finished! check if there are more to send if (client->num_queued_messages > 0) { // grab a message from the front of our message queue - client->queued_message = client->queued_messages[0].data; + client->queued_message = client->queued_messages[0].msg; client->queued_message_written = 0; - client->queued_message_length = client->queued_messages[0].length; + client->queued_message_length = client->queued_messages[0].msg->len + sizeof(struct ps_msg_header); client->num_queued_messages -= 1; if (client->num_queued_messages == 0) @@ -338,9 +344,9 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no // if we havent gotten a header yet, just check for that if (client->desired_packet_size == 0) { - const int header_size = 5; + const int header_size = sizeof(struct ps_msg_header); int len = recv(client->socket, buf, header_size, MSG_PEEK); - //printf("recv %i desired size 0\n", len); + //printf("peek %i desired size %i\n", len, header_size); if (len == 0) { client->needs_removal = true; @@ -406,12 +412,13 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no impl->clients[i].publisher = pub; // send the client the acknowledgement and message definition - int8_t packet_type = 0x03;//message definition - send(impl->clients[i].socket, (char*)&packet_type, 1, 0); - char buf[1500]; int32_t length = ps_serialize_message_definition((void*)buf, pub->message_definition); - send(impl->clients[i].socket, (char*)&length, 4, 0); + struct ps_msg_header hdr; + hdr.pid = PS_TCP_PROTOCOL_MESSAGE_DEFINITION;// message definition + hdr.length = length; + hdr.id = hdr.seq = 0; + send(impl->clients[i].socket, (char*)&hdr, sizeof(hdr), 0); send(impl->clients[i].socket, buf, length, 0); #ifdef PUBSUB_VERBOSE @@ -464,18 +471,18 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no // make the subscribe request in a "packet" // a packet is an int length followed by data - int8_t packet_type = 0x01;//subscribe - send(connection->socket, (char*)&packet_type, 1, 0); - int32_t length = strlen(connection->subscriber->topic) + 1 + 4; - send(connection->socket, (char*)&length, 4, 0); + + struct ps_msg_header hdr; + hdr.pid = 0x01; + hdr.length = length; + hdr.id = hdr.seq = 0; + send(connection->socket, (char*)&hdr, sizeof(hdr), 0); // make the request - char buffer[500]; - strcpy(buffer, connection->subscriber->topic); uint32_t skip = connection->subscriber->skip; send(connection->socket, (char*)&skip, 4, 0); - send(connection->socket, buffer, length - 4, 0); + send(connection->socket, connection->subscriber->topic, length - 4, 0); connection->connecting = false; } @@ -483,9 +490,8 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no // if we havent gotten a header yet, just check for that else if (connection->waiting_for_header) { - const int header_size = 5; + const int header_size = sizeof(struct ps_msg_header); int len = recv(connection->socket, buf, header_size, MSG_PEEK); - //printf("len %i\n", len); //printf("peek got: %i\n", len); if (len == 0) { @@ -533,7 +539,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no if (connection->current_size == connection->packet_size) { //printf("message finished type %x\n", connection->packet_type); - if (connection->packet_type == 0x3) + if (connection->packet_type == PS_TCP_PROTOCOL_MESSAGE_DEFINITION) { //printf("Was message definition\n"); if (connection->subscriber->type == 0) @@ -553,7 +559,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no free(connection->packet_data); } - else if (connection->packet_type == 0x2) + else if (connection->packet_type == PS_TCP_PROTOCOL_DATA) { //printf("added to queue\n"); // decode and add it to the queue @@ -595,8 +601,11 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no return message_count; } -void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, const void* message, uint32_t length) +void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, struct ps_msg_ref_t* msg) { + // todo dont + int length = msg->len; + void* message = msg->data; struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; // the client packs the socket id in the addr @@ -619,21 +628,17 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub { // check if we have queue space left - // for now hardcode max queue size + // for now hardcode max queue size const int max_queue_size = 10; - // copy the message to put it in the queue - // todo remove this copy - char* data = (char*)malloc(length + 4 + 1); - data[0] = 0x02; - *((uint32_t*)&data[1]) = length; - memcpy(&data[5], ps_get_msg_start(message), length); + // add a reference to the message and queue it up + ps_msg_ref_add(msg); // this if statement is unnecessary, but I added it for the sake of testing/completeness if (tclient->queued_message == 0) { - tclient->queued_message = data; - tclient->queued_message_length = length + 5; + tclient->queued_message = msg; + tclient->queued_message_length = length + sizeof(struct ps_msg_header); tclient->queued_message_written = 0; } else if (tclient->num_queued_messages >= max_queue_size) @@ -644,8 +649,7 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub { tclient->queued_messages[i] = tclient->queued_messages[i - 1]; } - tclient->queued_messages[0].data = data; - tclient->queued_messages[0].length = length + 5; + tclient->queued_messages[0].msg = msg; printf("dropped message on topic '%s'\n", publisher->topic); return;// drop it, we are out of queue space } @@ -657,8 +661,7 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub tclient->num_queued_messages += 1; struct ps_tcp_client_queued_message_t* msgs = (struct ps_tcp_client_queued_message_t*)malloc(tclient->num_queued_messages * sizeof(struct ps_tcp_client_queued_message_t)); - msgs[0].data = data; - msgs[0].length = length + 5; + msgs[0].msg = msg; for (int i = 0; i < tclient->num_queued_messages - 1; i++) { msgs[i + 1] = tclient->queued_messages[i]; @@ -671,11 +674,14 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub } //printf("started writing\n"); // try and write, if any of these fail, make a copy - uint8_t packet_type = 0x02; - int c = send(socket, (char*)&packet_type, 1, 0); - if (c == 0) + + // the message header is already filled out with the packet id and length + + int32_t desired_len = sizeof(struct ps_msg_header) + length; + int32_t c = send(socket, message, desired_len, 0); + if (c < desired_len && c >= 0) { - tclient->queued_message_written = 0; + tclient->queued_message_written = c; goto FAILCOPY; } if (c < 0) @@ -693,48 +699,6 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub goto FAILDISCONNECT; } - c = send(socket, (char*)&length, 4, 0); - if (c < 4 && c >= 0) - { - tclient->queued_message_written = c + 1; - goto FAILCOPY; - } - if (c < 0) - { -#ifdef WIN32 - int error = WSAGetLastError(); - if (error == WSAEWOULDBLOCK) -#else - if (errno == EAGAIN || errno == EWOULDBLOCK) -#endif - { - tclient->queued_message_written = 1; - goto FAILCOPY; - } - goto FAILDISCONNECT; - } - - //printf("sending %i bytes\n", length + 4 + 1); - c = send(socket, (char*)ps_get_msg_start(message), length, 0); - if (c < length && c >= 0) - { - tclient->queued_message_written = c + 5; - goto FAILCOPY; - } - if (c < 0) - { -#ifdef WIN32 - int error = WSAGetLastError(); - if (error == WSAEWOULDBLOCK) -#else - if (errno == EAGAIN || errno == EWOULDBLOCK) -#endif - { - tclient->queued_message_written = 5; - goto FAILCOPY; - } - goto FAILDISCONNECT; - } //printf("wrote all\n"); return; @@ -745,14 +709,11 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub return; FAILCOPY: - // todo remove this copy - data = (char*)malloc(length + 4 + 1); - data[0] = 0x02; - *((uint32_t*)&data[1]) = length; - memcpy(&data[5], ps_get_msg_start(message), length); - - tclient->queued_message = data; - tclient->queued_message_length = length + 5; + // add a reference count and put it in our queue + ps_msg_ref_add(msg); + + tclient->queued_message = msg; + tclient->queued_message_length = length + sizeof(struct ps_msg_header); ps_event_set_add_socket_write(&publisher->node->events, socket); return; } diff --git a/include/pubsub/UDPTransport.h b/include/pubsub/UDPTransport.h index cf32d88..ac27fb9 100644 --- a/include/pubsub/UDPTransport.h +++ b/include/pubsub/UDPTransport.h @@ -17,12 +17,13 @@ struct ps_endpoint_t; struct ps_sub_t; struct ps_pub_t; struct ps_msg_t; +struct ps_msg_ref_t; struct ps_client_t; void ps_udp_subscribe(struct ps_sub_t* sub, const struct ps_endpoint_t* ep); void ps_udp_unsubscribe(struct ps_sub_t* sub); -void ps_udp_publish(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_t* msg); +void ps_udp_publish(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_ref_t* msg); #endif diff --git a/src/Node.c b/src/Node.c index 86e57a7..017a65e 100644 --- a/src/Node.c +++ b/src/Node.c @@ -154,8 +154,7 @@ void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, cons pub->topic = topic; pub->node = node; pub->latched = latched; - pub->last_message.data = 0; - pub->last_message.len = 0; + pub->last_message = 0; pub->sequence_number = 0; pub->recommended_transport = recommended_transport; diff --git a/src/Publisher.c b/src/Publisher.c index 93799e2..11cc6f8 100644 --- a/src/Publisher.c +++ b/src/Publisher.c @@ -10,7 +10,7 @@ #include -void ps_pub_publish_client(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_t* msg) +void ps_pub_publish_client(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_ref_t* msg) { // Skip messages if desired by the client if (client->modulo > 0) @@ -21,12 +21,12 @@ void ps_pub_publish_client(struct ps_pub_t* pub, struct ps_client_t* client, str } } - if (client->transport) - { - //printf("publishing to custom transport\n"); - client->transport->pub(client->transport, pub, client, msg->data, msg->len); - return; - } + if (client->transport) + { + //printf("publishing to custom transport\n"); + client->transport->pub(client->transport, pub, client, msg); + return; + } // Send it via UDP transport ps_udp_publish(pub, client, msg); @@ -63,15 +63,19 @@ bool ps_pub_add_client(struct ps_pub_t* pub, const struct ps_client_t* client) pub->clients[i] = old_clients[i]; } pub->clients[pub->num_clients - 1] = *client; - + if (old_clients) + { + free(old_clients); + } + // todo this is probably the wrong spot for this // If we are latched, send the new client our last message - if (pub->last_message.data && pub->latched) + if (pub->last_message && pub->latched) { //printf("publishing latched\n"); - ps_pub_publish_client(pub, &pub->clients[pub->num_clients - 1], &pub->last_message); + ps_pub_publish_client(pub, &pub->clients[pub->num_clients - 1], pub->last_message); } - return true; + return true; } void ps_pub_add_endpoint_client(struct ps_pub_t* pub, const struct ps_endpoint_t* endpoint, const unsigned int stream_id) @@ -126,6 +130,10 @@ void ps_pub_remove_client(struct ps_pub_t* pub, const struct ps_client_t* client pub->clients[pos++] = old_clients[i]; } } + if (old_clients) + { + free(old_clients); + } } void ps_pub_publish_ez(struct ps_pub_t* pub, void* msg) @@ -141,26 +149,46 @@ void ps_pub_publish_ez(struct ps_pub_t* pub, void* msg) void ps_pub_publish(struct ps_pub_t* pub, struct ps_msg_t* msg) { pub->sequence_number++; + + // exit early if not latched and no clients + if (pub->num_clients == 0 && !pub->latched) + { + free(msg->data);// todo allocator + return; + } + // transfer it to a reference + struct ps_msg_ref_t* ref = (struct ps_msg_ref_t*)malloc(sizeof(struct ps_msg_ref_t)); + ref->len = msg->len; + ref->data = msg->data; + ref->refcount = 1; + + // fill out the header + struct ps_msg_header* hdr = (struct ps_msg_header*)msg->data; + hdr->pid = PS_UDP_PROTOCOL_DATA; + hdr->length = msg->len; + hdr->seq = pub->sequence_number; + hdr->id = 0; + for (unsigned int i = 0; i < pub->num_clients; i++) { struct ps_client_t* client = &pub->clients[i]; - ps_pub_publish_client(pub, client, msg); + ps_pub_publish_client(pub, client, ref); } if (pub->latched) { - if (pub->last_message.data) + if (pub->last_message) { //free the old and add the new - free(pub->last_message.data);// todo use allocator + ps_msg_ref_free(pub->last_message);// todo use allocator } - pub->last_message = *msg; + pub->last_message = ref; } else { - free(msg->data);// todo use allocator + ps_msg_ref_free(ref);// todo use allocator } } @@ -176,9 +204,9 @@ void ps_pub_destroy(struct ps_pub_t* pub) //remove it from the node's list of pubs pub->node->num_pubs--; struct ps_pub_t** old_pubs = pub->node->pubs; - if (pub->node->num_pubs) - { - pub->node->pubs = (struct ps_pub_t**)malloc(sizeof(struct ps_pub_t*)*pub->node->num_pubs); + if (pub->node->num_pubs) + { + pub->node->pubs = (struct ps_pub_t**)malloc(sizeof(struct ps_pub_t*)*pub->node->num_pubs); int ind = 0; for (unsigned int i = 0; i < pub->node->num_pubs+1; i++) { @@ -191,17 +219,17 @@ void ps_pub_destroy(struct ps_pub_t* pub) pub->node->pubs[ind++] = old_pubs[i]; } } - } - else - { - pub->node->pubs = 0; - } + } + else + { + pub->node->pubs = 0; + } free(old_pubs); - // free my latched message - if (pub->last_message.data) + // free my latched message + if (pub->last_message) { - free(pub->last_message.data);// todo use allocator + ps_msg_ref_free(pub->last_message);// todo use allocator } pub->clients = 0; diff --git a/src/Serialization.c b/src/Serialization.c index 814fb6b..3bc9f37 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -612,14 +612,29 @@ void ps_print_definition(const struct ps_message_definition_t* definition, bool void ps_msg_alloc(unsigned int size, struct ps_allocator_t* allocator, struct ps_msg_t* out_msg) { out_msg->len = size; - if (allocator) - { - out_msg->data = (void*)allocator->alloc(size + sizeof(struct ps_msg_header), allocator->context); - } - else - { - out_msg->data = (void*)((char*)malloc(size + sizeof(struct ps_msg_header))); - } + if (allocator) + { + out_msg->data = (void*)allocator->alloc(size + sizeof(struct ps_msg_header), allocator->context); + } + else + { + out_msg->data = (void*)((char*)malloc(size + sizeof(struct ps_msg_header))); + } +} + +void ps_msg_ref_add(struct ps_msg_ref_t* msg) +{ + msg->refcount++; +} + +void ps_msg_ref_free(struct ps_msg_ref_t* msg) +{ + msg->refcount--; + if (msg->refcount == 0) + { + free(msg->data); + free(msg); + } } diff --git a/src/UDPTransport.c b/src/UDPTransport.c index 65a655f..1efba38 100644 --- a/src/UDPTransport.c +++ b/src/UDPTransport.c @@ -4,7 +4,7 @@ #include -void ps_udp_publish(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_t* msg) +void ps_udp_publish(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_ref_t* msg) { // send da udp packet! struct sockaddr_in address; @@ -16,10 +16,9 @@ void ps_udp_publish(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_ //need to add in the topic id struct ps_msg_header* hdr = (struct ps_msg_header*)msg->data; hdr->pid = PS_UDP_PROTOCOL_DATA; + hdr->length = msg->len; hdr->id = client->stream_id; hdr->seq = client->sequence_number++; - hdr->index = 0; - hdr->count = 1;// todo use me for larger packets int sent_bytes = sendto(pub->node->socket, (const char*)msg->data, msg->len + sizeof(struct ps_msg_header), 0, (struct sockaddr*)&address, sizeof(struct sockaddr_in)); From 4cc0ab3b48a5b3426066b066a22f0be4ac9e5e09 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 27 Oct 2024 14:37:40 -0700 Subject: [PATCH 09/34] Fix tcp transport queuing Add test for very large messages --- include/pubsub/TCPTransport.h | 13 ++++++-- tests/test_pubsub_c.cpp | 59 +++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 3 deletions(-) diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 675dc5b..9ea8e5d 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -281,7 +281,8 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no while (client->queued_message != 0) { int to_send = client->queued_message_length - client->queued_message_written; - int sent = send(client->socket, &client->queued_message[client->queued_message_written], to_send, 0); + uint8_t* data = (uint8_t*)client->queued_message->data; + int sent = send(client->socket, &data[client->queued_message_written], to_send, 0); if (sent > 0) { client->queued_message_written += sent; @@ -292,8 +293,11 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no else if (sent < 0 && errno != EAGAIN) #endif { + //printf("needs removal %i\n", errno); client->needs_removal = true; } + + //printf("Sending more: %i to make %i of %i\n", sent, client->queued_message_written, client->queued_message_length); if (client->queued_message_written == client->queued_message_length) { @@ -533,9 +537,9 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no } else if (len > 0) { - //printf("Read %i bytes of message\n", len); connection->current_size += len; - + //printf("Read %i bytes of message, so far: %i\n", len, connection->current_size); + if (connection->current_size == connection->packet_size) { //printf("message finished type %x\n", connection->packet_type); @@ -678,6 +682,7 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub // the message header is already filled out with the packet id and length int32_t desired_len = sizeof(struct ps_msg_header) + length; + //printf("trying to send message of %i bytes\n", desired_len); int32_t c = send(socket, message, desired_len, 0); if (c < desired_len && c >= 0) { @@ -712,6 +717,8 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub // add a reference count and put it in our queue ps_msg_ref_add(msg); + //printf("Wrote %i bytes\n", tclient->queued_message_written); + tclient->queued_message = msg; tclient->queued_message_length = length + sizeof(struct ps_msg_header); ps_event_set_add_socket_write(&publisher->node->events, socket); diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index 2b37b45..745f639 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -6,6 +6,7 @@ #include #include +#include #include "mini_mock.hpp" /*add test to make sure mismatched messages are detected and not received @@ -205,4 +206,62 @@ TEST(test_publish_subscribe_latched_cb_broadcast_tcp, []() { latch_test_cb(true, true); }); +// test sending a very large message +TEST(test_publish_subscribe_large, []() { + struct ps_node_t node; + ps_node_init(&node, "test_node", "", true); + + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, &node); + ps_node_add_transport(&node, &tcp_transport); + + struct ps_pub_t string_pub; + ps_node_create_publisher(&node, "/data", &pubsub__PointCloud_def, &string_pub, true); + + // come up with the latched topic + static struct pubsub__PointCloud rmsg; + rmsg.num_points = 100000000;// 10 million points! + rmsg.point_type = pubsub::msg::PointCloud::POINT_XYZ; + rmsg.data_length = rmsg.num_points*4*3;//3 floats per point + rmsg.data = (uint8_t*)malloc(rmsg.data_length); + ps_pub_publish_ez(&string_pub, &rmsg); + + struct ps_sub_t string_sub; + + struct ps_subscriber_options options; + ps_subscriber_options_init(&options); + //options.skip = skip; + options.queue_size = 0; + options.allocator = 0; + options.ignore_local = false; + + static bool got_message = false; + options.preferred_transport = 1; + options.cb = [](void* message, unsigned int size, void* data2, const ps_msg_info_t* info) + { + got_message = true; + printf("Got message\n"); + // todo need to also assert we have the message type + // which is tricky for udp... + auto data = (struct pubsub__PointCloud*)pubsub__PointCloud_decode(message, &ps_default_allocator); + printf("Decoded message\n"); + EXPECT(data->num_points == rmsg.num_points); + free(data->data); + free(data); + }; + ps_node_create_subscriber_adv(&node, "/data", 0, &string_sub, &options); + + // now spin and wait for us to get the published message + while (ps_okay() && !got_message) + { + ps_node_spin(&node);// todo blocking wait first + + ps_sleep(1); + } + +done: + EXPECT(got_message); + ps_node_destroy(&node); +}); + CREATE_MAIN_ENTRY_POINT(); From 018e07044df802170390e5333dea9552c5de521b Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 14 Feb 2025 21:15:10 -0800 Subject: [PATCH 10/34] Remove queues from the C api and allow getting serialized messages in callbacks Use allocators in more cases --- .gitignore | 1 + examples/simple_pub.c | 4 +- examples/simple_sub.c | 23 ++++---- include/pubsub/Node.h | 15 ++--- include/pubsub/Subscriber.h | 13 +---- include/pubsub/TCPTransport.h | 36 ++++-------- include/pubsub_cpp/Node.h | 6 -- include/pubsub_cpp/array_vector.h | 9 +++ src/Node.c | 68 ++-------------------- src/Subscriber.c | 96 +++++++++---------------------- tests/test_pubsub_c.cpp | 68 ---------------------- tools/PubSubTest.cpp | 15 +++-- tools/pubsub.cpp | 7 +-- 13 files changed, 82 insertions(+), 279 deletions(-) diff --git a/.gitignore b/.gitignore index 823ba94..9b631a8 100644 --- a/.gitignore +++ b/.gitignore @@ -272,3 +272,4 @@ tests/CMakeFiles/* Makefile cmake_install.cmake build/ +html/ diff --git a/examples/simple_pub.c b/examples/simple_pub.c index 79f4f76..fa269e6 100644 --- a/examples/simple_pub.c +++ b/examples/simple_pub.c @@ -26,10 +26,10 @@ int main() // Create the publisher struct ps_pub_t string_pub; - ps_node_create_publisher(&node, "/data"/*topic name*/, + ps_node_create_publisher_ex(&node, "/data"/*topic name*/, &pubsub__String_def/*message definition*/, &string_pub, - true/*true to "latch" the topic*/); + true/*true to "latch" the topic*/, 1); // User is responsible for lifetime of the message they publish // Publish does a copy internally if necessary diff --git a/examples/simple_sub.c b/examples/simple_sub.c index 8f7fae3..0d52f2c 100644 --- a/examples/simple_sub.c +++ b/examples/simple_sub.c @@ -9,6 +9,15 @@ #include +struct ps_sub_t string_sub; +void callback(void* message, unsigned int size, void* cbdata, const struct ps_msg_info_t* info) +{ + // user is responsible for freeing the message and its arrays + struct pubsub__String* data = (struct pubsub__String*)message; + printf("Got message: %s\n", data->value); + pubsub__String_free(string_sub.allocator, data); +} + int main() { // Create the node @@ -23,10 +32,10 @@ int main() ps_node_add_transport(&node, &tcp_transport); // Create the subscriber - struct ps_sub_t string_sub; struct ps_subscriber_options options; ps_subscriber_options_init(&options); options.preferred_transport = PUBSUB_TCP_TRANSPORT;// sets preferred transport to TCP + options.cb = callback; ps_node_create_subscriber_adv(&node, "/data", &pubsub__String_def, &string_sub, &options); // Loop and spin @@ -36,18 +45,8 @@ int main() // Used to prevent this from using 100% CPU, but you can do that through other means ps_node_wait(&node, 1000/*maximum wait time in ms*/); - // Updates the node, which will queue up any received messages + // Updates the node, which will receive messages and call any callbacks as they come in ps_node_spin(&node); - - // our sub has a message definition, so the queue contains real messages - struct pubsub__String* data; - while (data = (struct pubsub__String*)ps_sub_deque(&string_sub)) - { - // user is responsible for freeing the message and its arrays - printf("Got message: %s\n", data->value); - free(data->value); - free(data); - } } // Shutdown the node to free resources diff --git a/include/pubsub/Node.h b/include/pubsub/Node.h index 228582f..8a572f9 100644 --- a/include/pubsub/Node.h +++ b/include/pubsub/Node.h @@ -67,6 +67,7 @@ typedef void(*ps_param_confirm_cb_t)(const char* name, double value, void* data) struct ps_node_t { const char* name; + const char* description; unsigned int num_pubs; struct ps_pub_t** pubs; unsigned int num_subs; @@ -195,24 +196,18 @@ void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const s void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport); -void ps_node_create_subscriber(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, - struct ps_sub_t* sub, - unsigned int queue_size,//make >= 1 - struct ps_allocator_t* allocator,//give null to use default - bool ignore_local);// if ignore local is set, this node ignores publications from itself - // this facilitiates passing messages through shared memory - typedef void(*ps_subscriber_fn_cb_t)(void* message, unsigned int size, void* data, const struct ps_msg_info_t* info); struct ps_subscriber_options { - unsigned int queue_size; bool ignore_local; struct ps_allocator_t* allocator; unsigned int skip;// skips to every nth message for throttling ps_subscriber_fn_cb_t cb; + ps_subscriber_fn_cb_t cb_raw; void* cb_data; - int32_t preferred_transport;// falls back to udp otherwise + int32_t preferred_transport;// falls back to udp otherwise + const char* description; }; void ps_subscriber_options_init(struct ps_subscriber_options* options); @@ -221,7 +216,7 @@ void ps_node_create_subscriber_adv(struct ps_node_t* node, const char* topic, co struct ps_sub_t* sub, const struct ps_subscriber_options* options); -void ps_node_create_subscriber_cb(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, +void ps_node_create_subscriber(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_sub_t* sub, ps_subscriber_fn_cb_t cb, void* cb_data, diff --git a/include/pubsub/Subscriber.h b/include/pubsub/Subscriber.h index 5dbd7d5..549bc58 100644 --- a/include/pubsub/Subscriber.h +++ b/include/pubsub/Subscriber.h @@ -31,19 +31,13 @@ struct ps_sub_t struct ps_allocator_t* allocator; - // used instead of a queue optionally ps_subscriber_fn_cb_t cb; + ps_subscriber_fn_cb_t cb_raw; void* cb_data; int preferred_transport;// udp or tcp, or -1 for no preference unsigned int skip; - - // queue is implemented as a deque - int queue_start;// start index of items in the queue (loops around on positive side) - int queue_size;// maximum size of the queue - int queue_len;// current queue size - void** queue;// pointers to each of the queue items }; #pragma pack(push) @@ -58,10 +52,7 @@ struct ps_sub_req_header_t }; #pragma pack(pop) -void ps_sub_enqueue(struct ps_sub_t* sub, void* message, int data_size, const struct ps_msg_info_t* message_info); - -// if the subscriber was initialized with a type this returns decoded messages -void* ps_sub_deque(struct ps_sub_t* sub); +void ps_sub_receive(struct ps_sub_t* sub, void* encoded_message, int data_size, bool is_reference, const struct ps_msg_info_t* message_info); void ps_sub_destroy(struct ps_sub_t* sub); diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 9ea8e5d..1280650 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -215,7 +215,6 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no } struct ps_tcp_client_t* new_client = &impl->clients[impl->num_clients - 1]; new_client->socket = socket; - new_client->socket = socket; new_client->needs_removal = false; new_client->current_packet_size = 0; new_client->desired_packet_size = 0; @@ -517,7 +516,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no connection->waiting_for_header = false; connection->packet_size = *(uint32_t*)&buf[1]; //printf("Incoming message with %i bytes\n", impl->connections[i].packet_size); - connection->packet_data = (char*)malloc(connection->packet_size); + connection->packet_data = (char*)connection->subscriber->allocator->alloc(connection->packet_size, connection->subscriber->allocator->context); connection->current_size = 0; } @@ -561,7 +560,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no } } - free(connection->packet_data); + connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); } else if (connection->packet_type == PS_TCP_PROTOCOL_DATA) { @@ -570,32 +569,17 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no struct ps_msg_info_t message_info; message_info.address = connection->endpoint.address; message_info.port = connection->endpoint.port; - - void* out_data; - if (connection->subscriber->type) - { - out_data = connection->subscriber->type->decode(connection->packet_data, connection->subscriber->allocator); - free(connection->packet_data); - } - else - { - out_data = connection->packet_data; - } - - // remove the reference to packet data so we dont try and double free it on destroy - // it is the queue's responsibility now + + ps_sub_receive(connection->subscriber, connection->packet_data, connection->packet_size, false, &message_info); + + // remove the reference to packet data so we don't try and double free it on destroy connection->packet_data = 0; - ps_sub_enqueue(connection->subscriber, - out_data, - connection->packet_size, - &message_info); - message_count++; } else { // unhandled packet id - free(connection->packet_data); + connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); } connection->waiting_for_header = true; } @@ -860,7 +844,8 @@ void ps_tcp_transport_unsubscribe(struct ps_transport_t* transport, struct ps_su if (!impl->connections[i].waiting_for_header) { - free(impl->connections[i].packet_data); + struct ps_sub_t* sub = impl->connections[i].subscriber; + sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); } ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); #ifdef _WIN32 @@ -885,7 +870,8 @@ void ps_tcp_transport_destroy(struct ps_transport_t* transport) { if (!impl->connections[i].waiting_for_header) { - free(impl->connections[i].packet_data); + struct ps_sub_t* sub = impl->connections[i].subscriber; + sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); } ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); #ifdef _WIN32 diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index af862d7..bfbff1d 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -572,7 +572,6 @@ class Subscriber: public SubscriberBase struct ps_subscriber_options options; ps_subscriber_options_init(&options); - options.queue_size = 0; options.cb = cb2; options.cb_data = this; options.allocator = 0; @@ -639,11 +638,6 @@ class Subscriber: public SubscriberBase node_ = 0; } - T* deque() - { - return (T*)ps_sub_deque(&subscriber_); - } - const std::string& getQualifiedTopic() { return remapped_topic_; diff --git a/include/pubsub_cpp/array_vector.h b/include/pubsub_cpp/array_vector.h index 84e5e03..426646f 100644 --- a/include/pubsub_cpp/array_vector.h +++ b/include/pubsub_cpp/array_vector.h @@ -93,6 +93,15 @@ class ArrayVector } data_ = new_data; } + + // reliquinquishes the held pointer without freeing + T* reset() + { + auto out = data_; + data_ = 0; + length_ = 0; + return out; + } void clear() { diff --git a/src/Node.c b/src/Node.c index 017a65e..73de257 100644 --- a/src/Node.c +++ b/src/Node.c @@ -522,11 +522,11 @@ struct ps_allocator_t ps_default_allocator = { ps_malloc_alloc, ps_malloc_free, void ps_subscriber_options_init(struct ps_subscriber_options* options) { - options->queue_size = 1; options->ignore_local = false; options->allocator = 0; options->skip = 0; options->cb = 0; + options->cb_raw = 0; options->cb_data = 0; options->preferred_transport = -1;// no preference } @@ -564,57 +564,15 @@ void ps_node_create_subscriber_adv(struct ps_node_t* node, const char* topic, co sub->received_message_def.hash = 0; sub->received_message_def.num_fields = 0; - // force queue size to be > 0 - unsigned int queue_size = options->queue_size; - if (options->cb) - { - sub->cb = options->cb; - sub->cb_data = options->cb_data; - sub->queue_size = 0; - sub->queue_len = 0; - sub->queue_start = 0; - sub->queue = 0; - } - else - { - if (queue_size <= 0) - { - queue_size = 1; - } - - // allocate queue data - sub->queue_len = 0; - sub->queue_start = 0; - sub->queue_size = queue_size; - sub->queue = (void**)malloc(sizeof(void*) * queue_size); - - for (unsigned int i = 0; i < queue_size; i++) - { - sub->queue[i] = 0; - } - } + sub->cb = options->cb; + sub->cb_raw = options->cb_raw; + sub->cb_data = options->cb_data; // send out the subscription query while we are at it ps_node_subscribe_query(sub); } void ps_node_create_subscriber(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, - struct ps_sub_t* sub, - unsigned int queue_size, - struct ps_allocator_t* allocator, - bool ignore_local) -{ - struct ps_subscriber_options options; - ps_subscriber_options_init(&options); - - options.queue_size = queue_size; - options.allocator = allocator; - options.ignore_local = ignore_local; - - ps_node_create_subscriber_adv(node, topic, type, sub, &options); -} - -void ps_node_create_subscriber_cb(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_sub_t* sub, ps_subscriber_fn_cb_t cb, void* cb_data, @@ -625,7 +583,6 @@ void ps_node_create_subscriber_cb(struct ps_node_t* node, const char* topic, con struct ps_subscriber_options options; ps_subscriber_options_init(&options); - options.queue_size = 0; options.cb = cb; options.cb_data = cb_data; options.allocator = allocator; @@ -822,22 +779,7 @@ int ps_node_spin(struct ps_node_t* node) // queue up the data, and copy :/ (can make zero copy for arduino version) int data_size = received_bytes - sizeof(struct ps_msg_header); - // also todo fastpath for PoD message types - - // okay, if we have the message definition, deserialize and output in a message - void* out_data; - if (sub->type) - { -//theres a leak if you use this and the queue fills up with complex types - out_data = sub->type->decode(data + sizeof(struct ps_msg_header), sub->allocator); - } - else - { - out_data = sub->allocator->alloc(data_size, sub->allocator->context); - memcpy(out_data, data + sizeof(struct ps_msg_header), data_size); - } - - ps_sub_enqueue(sub, out_data, data_size, &message_info); + ps_sub_receive(sub, data + sizeof(struct ps_msg_header), data_size, true, &message_info); #ifdef PUBSUB_VERBOSE //printf("Got message, queue len %i\n", sub->queue_len); diff --git a/src/Subscriber.c b/src/Subscriber.c index 3ea0c4d..241baeb 100644 --- a/src/Subscriber.c +++ b/src/Subscriber.c @@ -7,45 +7,42 @@ #ifndef ANDROID #include #endif +#include #include -void ps_sub_enqueue(struct ps_sub_t* sub, void* data, int data_size, const struct ps_msg_info_t* message_info) +void ps_sub_receive(struct ps_sub_t* sub, void* encoded_message, int data_size, bool is_reference, const struct ps_msg_info_t* message_info) { - // Implement a LIFO queue. Is this the best option? - int new_start = sub->queue_start - 1; - if (new_start < 0) - { - new_start += sub->queue_size; - } + // if is_reference is true, we must make a copy for the subscriber to own + // okay, so how do we let the callback specify if it wants a copy or not? - // If no queue size, just run the callback immediately - if (sub->queue_size == 0) + //todo make this not always require owning the data + //how do I release a "loaned" message? also, how do I loan? + if (sub->cb_raw) { - sub->cb(data, data_size, sub->cb_data, message_info); - } - // Handle replacement if the queue is full - else if (sub->queue_size == sub->queue_len) - { - // we'll replace the item at the back by shifting the queue around - if (sub->type) + // todo can avoid the copy if the allocator is used + void* out_data; + if (is_reference) { - sub->type->free(sub->allocator, sub->queue[sub->queue_start]); + out_data = sub->allocator->alloc(data_size, sub->allocator->context); + memcpy(out_data, encoded_message, data_size); } else { - free(sub->queue[sub->queue_start]); - } - // add at the front - sub->queue[new_start] = data; - sub->queue_start = new_start; + out_data = encoded_message; + } + sub->cb_raw(out_data, data_size, sub->cb_data, message_info); } - else + + if (sub->cb) + { + void* out_data = sub->type->decode(encoded_message, sub->allocator); + sub->cb(out_data, data_size, sub->cb_data, message_info); + } + + if (!is_reference && !sub->cb_raw) { - // add to the front - sub->queue_len++; - sub->queue[new_start] = data; - sub->queue_start = new_start; + sub->allocator->free(encoded_message, sub->allocator->context); } } @@ -58,7 +55,7 @@ void ps_sub_destroy(struct ps_sub_t* sub) for (unsigned int i = 0; i < sub->node->num_transports; i++) { sub->node->transports[i].unsubscribe(&sub->node->transports[i], sub); - } + } //remove it from my list of subs sub->node->num_subs--; @@ -84,46 +81,5 @@ void ps_sub_destroy(struct ps_sub_t* sub) } } free(old_subs); - } - - // free any queued up received messages and the queue itself - for (int i = 0; i < sub->queue_len; i++) - { - int index = (sub->queue_start + i)%sub->queue_size; - if (sub->queue[index] != 0) - { - if (sub->type) - { - sub->type->free(sub->allocator, sub->queue[index]); - } - else - { - free(sub->queue[index]); - } - } - } - free(sub->queue); -} - -void* ps_sub_deque(struct ps_sub_t* sub) -{ - if (sub->queue_len == 0) - { - //printf("Warning: dequeued when there was nothing in queue\n"); - return 0; - } - - // we are dequeueing, so remove the newest first (from the front) - sub->queue_len--; - - void* data = sub->queue[sub->queue_start]; - sub->queue[sub->queue_start] = 0; - - int new_start = sub->queue_start+1; - if (new_start >= sub->queue_size) - { - new_start -= sub->queue_size; - } - sub->queue_start = new_start; - return data; + } } diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index 745f639..9225ff7 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -45,7 +45,6 @@ TEST(test_publish_subscribe_generic, []() { struct ps_subscriber_options options; ps_subscriber_options_init(&options); //options.skip = skip; - options.queue_size = 0; options.allocator = 0; options.ignore_local = false; @@ -77,72 +76,6 @@ TEST(test_publish_subscribe_generic, []() { ps_node_destroy(&node); }); -void latch_test(bool broadcast, bool tcp) -{ - struct ps_node_t node; - ps_node_init(&node, "test_node", "", broadcast); - - struct ps_transport_t tcp_transport; - ps_tcp_transport_init(&tcp_transport, &node); - ps_node_add_transport(&node, &tcp_transport); - - struct ps_pub_t string_pub; - ps_node_create_publisher(&node, "/data", &pubsub__String_def, &string_pub, true); - - struct ps_sub_t string_sub; - - struct ps_subscriber_options options; - ps_subscriber_options_init(&options); - options.preferred_transport = tcp ? 1 : 0;// tcp yo - ps_node_create_subscriber_adv(&node, "/data", &pubsub__String_def, &string_sub, &options); - - // come up with the latched topic - struct pubsub__String rmsg; - rmsg.value = "Hello"; - ps_pub_publish_ez(&string_pub, &rmsg); - - bool got_message = false; - - // now spin and wait for us to get the published message - while (ps_okay()) - { - ps_node_spin(&node);// todo blocking wait first - - struct pubsub__String* data; - while (data = (struct pubsub__String*)ps_sub_deque(&string_sub)) - { - // user is responsible for freeing the message and its arrays - printf("Got message: %s\n", data->value); - EXPECT(strcmp(data->value, rmsg.value) == 0); - got_message = true; - free(data->value); - free(data);//todo use allocator free - goto done; - } - ps_sleep(1); - } - -done: - EXPECT(got_message); - ps_node_destroy(&node); -} - -TEST(test_publish_subscribe_latched_multicast, []() { - latch_test(false, false); -}); - -TEST(test_publish_subscribe_latched_broadcast, []() { - latch_test(true, false); -}); - -TEST(test_publish_subscribe_latched_multicast_tcp, []() { - latch_test(false, true); -}); - -TEST(test_publish_subscribe_latched_broadcast_tcp, []() { - latch_test(true, true); -}); - void latch_test_cb(bool broadcast, bool tcp) { struct ps_node_t node; @@ -231,7 +164,6 @@ TEST(test_publish_subscribe_large, []() { struct ps_subscriber_options options; ps_subscriber_options_init(&options); //options.skip = skip; - options.queue_size = 0; options.allocator = 0; options.ignore_local = false; diff --git a/tools/PubSubTest.cpp b/tools/PubSubTest.cpp index 05cb4ee..cc77a3c 100644 --- a/tools/PubSubTest.cpp +++ b/tools/PubSubTest.cpp @@ -23,7 +23,13 @@ int main() ps_node_create_publisher(&node, "/joy", &pubsub__Joy_def, &adv_pub, false); ps_sub_t string_sub; - ps_node_create_subscriber(&node, "/data", &pubsub__String_def, &string_sub, 10, 0, false); + auto cb = [](void* message, unsigned int size, void* cbdata, const ps_msg_info_t* info) + { + pubsub__String* data = (pubsub__String*)message; + free(data->value); + free(data);//todo use allocator free + }; + ps_node_create_subscriber(&node, "/data", &pubsub__String_def, &string_sub, cb, 0, 0, false); // wait until we get the subscription request while (ps_pub_get_subscriber_count(&string_pub) == 0) @@ -61,13 +67,6 @@ int main() ps_node_spin(&node); //while (ps_node_spin(&node) == 0) { Sleep(1); } - // our sub has a message definition, so the queue contains real messages - while (pubsub__String* data = (pubsub__String*)ps_sub_deque(&string_sub)) - { - printf("Got message: %s\n", data->value); - free(data->value); - free(data);//todo use allocator free - } printf("Num subs: %i %i\n", ps_pub_get_subscriber_count(&string_pub), ps_pub_get_subscriber_count(&adv_pub)); ps_sleep(1000); diff --git a/tools/pubsub.cpp b/tools/pubsub.cpp index 56f24f7..4cfa7d4 100644 --- a/tools/pubsub.cpp +++ b/tools/pubsub.cpp @@ -320,13 +320,12 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) struct ps_subscriber_options options; ps_subscriber_options_init(&options); options.skip = skip; - options.queue_size = 0; options.allocator = 0; options.ignore_local = false; options.preferred_transport = -1; options.preferred_transport = parser.GetBool("tcp") ? 1 : options.preferred_transport; options.preferred_transport = parser.GetBool("udp") ? 0 : options.preferred_transport; - options.cb = [](void* message, unsigned int size, void* data, const ps_msg_info_t* info) + options.cb_raw = [](void* message, unsigned int size, void* data, const ps_msg_info_t* info) { // get and deserialize the messages if (sub.received_message_def.fields == 0) @@ -357,7 +356,7 @@ int topic_echo(int num_args, char** args, ps_node_t* _node) } ps_deserialize_print(message, &sub.received_message_def, no_arr ? 10 : 0, field_name); printf("-------------\n"); - free(message); + free(message);// todo use allocator if (++count >= n) { // need to commit sudoku here.. @@ -889,7 +888,7 @@ int main(int num_args_real, char** args) ps_subscriber_options opts; ps_subscriber_options_init(&opts); - opts.cb = cb; + opts.cb_raw = cb; opts.cb_data = &message_times; opts.preferred_transport = -1; opts.preferred_transport = parser.GetBool("tcp") ? 1 : opts.preferred_transport; From ab3c7be0728d5041c87062295f0cf861ae124bac Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 14 Feb 2025 21:21:59 -0800 Subject: [PATCH 11/34] Fix some errors and warnings --- src/Parameter.c | 2 +- tools/generator.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/Parameter.c b/src/Parameter.c index 55ef4ce..4d5e760 100644 --- a/src/Parameter.c +++ b/src/Parameter.c @@ -1,6 +1,6 @@ #include -//#include +#include #include #include diff --git a/tools/generator.cpp b/tools/generator.cpp index a314f01..6af4674 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -587,7 +588,7 @@ std::string generate(const char* definition, const char* name) } output += " out->" + fields[i].name + "_length = num_" + fields[i].name + ";\n"; - output += " out->" + fields[i].name + " = (char**)malloc(sizeof(char*)*num_" + fields[i].name + ");\n"; + output += " out->" + fields[i].name + " = (char**)allocator->alloc(sizeof(char*)*num_" + fields[i].name + ", allocator->context);\n"; // allocate the array // need to do it! @@ -596,7 +597,7 @@ std::string generate(const char* definition, const char* name) output += " int len = *(uint32_t*)p;\n"; output += " p += 4;\n";// add size of length // now read and allocate each string - output += " out->" + fields[i].name + "[i] = (char*)malloc(len);\n"; + output += " out->" + fields[i].name + "[i] = (char*)allocator->alloc(len, allocator->context);\n"; output += " memcpy(out->" + fields[i].name + "[i], p, len);\n"; output += " p += len;\n"; output += " }\n"; From 60096733408a90ba16227939c106feb545f80f6e Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 14 Feb 2025 21:27:46 -0800 Subject: [PATCH 12/34] Fix some windows errors --- include/pubsub/TCPTransport.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 1280650..e7dca9c 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -280,7 +280,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no while (client->queued_message != 0) { int to_send = client->queued_message_length - client->queued_message_written; - uint8_t* data = (uint8_t*)client->queued_message->data; + char* data = (char*)client->queued_message->data; int sent = send(client->socket, &data[client->queued_message_written], to_send, 0); if (sent > 0) { @@ -667,7 +667,7 @@ void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* pub int32_t desired_len = sizeof(struct ps_msg_header) + length; //printf("trying to send message of %i bytes\n", desired_len); - int32_t c = send(socket, message, desired_len, 0); + int32_t c = send(socket, (char*)message, desired_len, 0); if (c < desired_len && c >= 0) { tclient->queued_message_written = c; From 016014b6438c43358bcf385697876b4d5f78e3fe Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 14 Feb 2025 21:36:57 -0800 Subject: [PATCH 13/34] Fix warnings and tests --- include/pubsub_cpp/array_vector.h | 4 ++-- tests/test_pubsub_c.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/pubsub_cpp/array_vector.h b/include/pubsub_cpp/array_vector.h index 426646f..001d003 100644 --- a/include/pubsub_cpp/array_vector.h +++ b/include/pubsub_cpp/array_vector.h @@ -68,7 +68,7 @@ class ArrayVector length_ = arr.size(); data_ = (T*)malloc(sizeof(T)*length_); - for (int i = 0; i < length_; i++) + for (uint32_t i = 0; i < length_; i++) { data_[i] = arr[i]; } @@ -82,7 +82,7 @@ class ArrayVector auto new_data = (T*)malloc(sizeof(T)*size); auto copy_len = std::min(size, length_); - for (int i = 0; i < copy_len; i++) + for (uint32_t i = 0; i < copy_len; i++) { new_data[i] = data_[i]; } diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index 9225ff7..bf4f5bf 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -50,7 +50,7 @@ TEST(test_publish_subscribe_generic, []() { static bool got_message = false; //options.preferred_transport = tcp ? 1 : 0; - options.cb = [](void* message, unsigned int size, void* data2, const ps_msg_info_t* info) + options.cb_raw = [](void* message, unsigned int size, void* data2, const ps_msg_info_t* info) { got_message = true; // todo need to also assert we have the message type @@ -60,6 +60,8 @@ TEST(test_publish_subscribe_generic, []() { EXPECT(strcmp(data->value, rmsg.value) == 0); free(data->value); free(data); + + free(message); }; ps_node_create_subscriber_adv(&node, "/data", 0, &string_sub, &options); @@ -169,7 +171,7 @@ TEST(test_publish_subscribe_large, []() { static bool got_message = false; options.preferred_transport = 1; - options.cb = [](void* message, unsigned int size, void* data2, const ps_msg_info_t* info) + options.cb_raw = [](void* message, unsigned int size, void* data2, const ps_msg_info_t* info) { got_message = true; printf("Got message\n"); @@ -180,6 +182,7 @@ TEST(test_publish_subscribe_large, []() { EXPECT(data->num_points == rmsg.num_points); free(data->data); free(data); + free(message); }; ps_node_create_subscriber_adv(&node, "/data", 0, &string_sub, &options); From 50a3cd9ccf4d951600485e1757f2771f954e61e1 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 14 Feb 2025 22:05:45 -0800 Subject: [PATCH 14/34] Add a message copy test and add a test timeout Re-enable tests --- .github/workflows/ccpp.yml | 4 ++-- tests/CMakeLists.txt | 1 + tests/test_pubsub_c.cpp | 6 ------ tests/test_serialization.cpp | 35 +++++++++++++++++++++++++++++------ 4 files changed, 32 insertions(+), 14 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 0aa62b2..e4b9fcc 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -20,5 +20,5 @@ jobs: run: cmake . - name: cmake build run: cmake --build . -# - name: Run Tests -# run: cd tests && ctest -VV --timeout 5 + - name: Run Tests + run: cd tests && ctest -VV diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6d3c88a..eccfded 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -17,5 +17,6 @@ foreach(test_file ${tests}) STRING(REGEX REPLACE ${TEST_REGEX} "\\1" test ${test}) message(STATUS " add test: ${test}") add_test(NAME ${test_file}_${test} COMMAND ${test_file} ${test}) + set_tests_properties(${test_file}_${test} PROPERTIES TIMEOUT 10) endforeach() endforeach() diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index bf4f5bf..ba465ac 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -13,17 +13,11 @@ also add a test to test subscriber/publisher numbers -add a way to add a timeout to tests - add a test for generic message handling make the pose viewer also be able to view odom in pubviz (maybe think of a way to view velocities)*/ -//lets test queue size too - -//so for that test, shove over N messages - TEST(test_publish_subscribe_generic, []() { struct ps_node_t node; ps_node_init(&node, "test_node", "", true); diff --git a/tests/test_serialization.cpp b/tests/test_serialization.cpp index eda4f6f..058323a 100644 --- a/tests/test_serialization.cpp +++ b/tests/test_serialization.cpp @@ -6,9 +6,8 @@ #include "mini_mock.hpp" TEST(test_joy_serialization, []() { - // try serializing then deserializing a message to make sure it all matches - pubsub::msg::Joy msg; + pubsub::msg::Joy msg; msg.buttons = 0x12345678; for (int i = 0; i < 8; i++) msg.axes[i] = i; @@ -29,7 +28,7 @@ TEST(test_joy_serialization, []() { void test2() { // verify that the C type matches the C++ one memory wise - pubsub::msg::Costmap msg; + pubsub::msg::Costmap msg; msg.width = 100; msg.height = 200; msg.resolution = 1.0; @@ -57,7 +56,7 @@ TEST(test_costmap_c_cpp, []() { TEST(test_costmap_serialization, []() { // try serializing then deserializing a message to make sure it all matches - pubsub::msg::Costmap msg; + pubsub::msg::Costmap msg; msg.width = 100; msg.height = 200; msg.resolution = 1.0; @@ -88,7 +87,7 @@ TEST(test_costmap_serialization, []() { TEST(test_path2d_serialization, []() { // try serializing then deserializing a message to make sure it all matches - pubsub::msg::Path2D msg; + pubsub::msg::Path2D msg; msg.frame = 100; msg.points.resize(123); for (int i = 0; i < msg.points.size(); i++) @@ -114,9 +113,33 @@ TEST(test_path2d_serialization, []() { delete out; }); +TEST(test_path2d_copy, []() { + // make sure copying a message works + pubsub::msg::Path2D msg; + msg.frame = 100; + msg.points.resize(123); + for (int i = 0; i < msg.points.size(); i++) + { + msg.points[i].x = i*2; + msg.points[i].y = i*2 + 1; + } + + pubsub::msg::Path2D msg2 = msg; + + EXPECT(msg2.frame == msg.frame); + EXPECT(msg2.points.size() == msg.points.size()); + for (int i = 0; i < msg.points.size(); i++) + { + if (msg2.points[i].x != msg.points[i].x) + EXPECT(false); + if (msg2.points[i].y != msg.points[i].y) + EXPECT(false); + } +}); + TEST(test_path2d_foreach, []() { // try serializing then deserializing a message, making sure the foreach loop over it works as expected - pubsub::msg::Path2D msg; + pubsub::msg::Path2D msg; msg.frame = 100; msg.points.resize(3); for (int i = 0; i < msg.points.size(); i++) From 62fe6cccb6f9e1ff0b97881ffe14c1dd9dd7c391 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 15:42:37 -0800 Subject: [PATCH 15/34] Check network config --- .github/workflows/ccpp.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index e4b9fcc..ea3cb0f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -20,5 +20,7 @@ jobs: run: cmake . - name: cmake build run: cmake --build . + - name: network + run: sudo apt install net-tools && ifconfig - name: Run Tests run: cd tests && ctest -VV From bd984f4a68ea7b272c9de86c2069b89645e5edc9 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 16:20:32 -0800 Subject: [PATCH 16/34] Discover broadcast address --- src/Node.c | 38 +++++++++++++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/src/Node.c b/src/Node.c index 73de257..ec21ef2 100644 --- a/src/Node.c +++ b/src/Node.c @@ -17,6 +17,13 @@ #include #endif +#ifndef _WIN32 +#include +#include +#include +#include +#endif + // sends out a system query message for all nodes to advertise void ps_node_system_query(struct ps_node_t* node) { @@ -316,9 +323,38 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b } else if (broadcast) { - //convert to a broadcast address (just the subnet wide one) + //convert to a broadcast address (just the subnet wide one) node->advertise_addr = inet_addr(ip); + //okay, for this to work we need the subnet address we're assuming and its sometimes wrong node->advertise_addr |= 0xFF000000; +#ifndef _WIN32 + ip = strdup(ip); + struct ifaddrs *ifap, *ifa; + struct sockaddr_in *sa; + char *addr; + + getifaddrs(&ifap); + for (ifa = ifap; ifa; ifa = ifa->ifa_next) + { + if (ifa->ifa_addr && ifa->ifa_addr->sa_family==AF_INET) + { + sa = (struct sockaddr_in*)ifa->ifa_addr; + if (sa->sin_addr.s_addr == inet_addr(ip)) + { + sa = (struct sockaddr_in*)ifa->ifa_ifu.ifu_broadaddr; + node->advertise_addr = sa->sin_addr.s_addr; + printf("found\n"); + } + } + } + + freeifaddrs(ifap); +#endif + // print the result + struct in_addr ip_addr; + ip_addr.s_addr = node->advertise_addr; + addr = inet_ntoa(ip_addr); + printf("Broadcast Address: %s\n", addr); } else { From a53e7832190bf70564975b34f40d202904b1e0ad Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 16:24:03 -0800 Subject: [PATCH 17/34] Fix windows --- src/Node.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/Node.c b/src/Node.c index ec21ef2..cad038f 100644 --- a/src/Node.c +++ b/src/Node.c @@ -328,10 +328,8 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b //okay, for this to work we need the subnet address we're assuming and its sometimes wrong node->advertise_addr |= 0xFF000000; #ifndef _WIN32 - ip = strdup(ip); struct ifaddrs *ifap, *ifa; struct sockaddr_in *sa; - char *addr; getifaddrs(&ifap); for (ifa = ifap; ifa; ifa = ifa->ifa_next) @@ -353,8 +351,7 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b // print the result struct in_addr ip_addr; ip_addr.s_addr = node->advertise_addr; - addr = inet_ntoa(ip_addr); - printf("Broadcast Address: %s\n", addr); + printf("Broadcast Address: %s\n", inet_ntoa(ip_addr)); } else { From 0d81430a9b4cb2bef4c84a1f5fdb580a3d69cbe6 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 16:44:39 -0800 Subject: [PATCH 18/34] Fix things --- src/Node.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/Node.c b/src/Node.c index cad038f..ffdd1c9 100644 --- a/src/Node.c +++ b/src/Node.c @@ -305,10 +305,11 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b { ip = GetPrimaryIp(); } + uint32_t our_address = inet_addr(ip); printf("Pubsub IP: %s\n", ip); #ifdef _WIN32 - node->group_id = GetCurrentProcessId() + (10000 * ((inet_addr(ip) >> 24) && 0xFF)); + node->group_id = GetCurrentProcessId() + (10000 * ((our_address >> 24) && 0xFF)); #else node->group_id = 0;// ignore the group #endif @@ -324,7 +325,7 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b else if (broadcast) { //convert to a broadcast address (just the subnet wide one) - node->advertise_addr = inet_addr(ip); + node->advertise_addr = our_address; //okay, for this to work we need the subnet address we're assuming and its sometimes wrong node->advertise_addr |= 0xFF000000; #ifndef _WIN32 @@ -334,10 +335,10 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b getifaddrs(&ifap); for (ifa = ifap; ifa; ifa = ifa->ifa_next) { - if (ifa->ifa_addr && ifa->ifa_addr->sa_family==AF_INET) + if (ifa->ifa_addr && ifa->ifa_addr->sa_family == AF_INET) { sa = (struct sockaddr_in*)ifa->ifa_addr; - if (sa->sin_addr.s_addr == inet_addr(ip)) + if (sa->sin_addr.s_addr == our_address) { sa = (struct sockaddr_in*)ifa->ifa_ifu.ifu_broadaddr; node->advertise_addr = sa->sin_addr.s_addr; @@ -363,7 +364,7 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b // Setup the core socket node->socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - node->addr = ntohl(inet_addr(ip)); + node->addr = ntohl(our_address); if (node->socket == 0) { printf("Failed To Create Socket!\n"); From b4618952b98d1f2fb4454c022b43c00ed4c223c3 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 22:22:05 -0800 Subject: [PATCH 19/34] Speed up CI --- .github/workflows/ccpp.yml | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index ea3cb0f..45f662d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -10,8 +10,15 @@ jobs: - name: cmake configure run: cmake . - name: cmake build - run: cmake --build . - + run: cmake --build . -j4 + mac-build: + runs-on: macos-latest + steps: + - uses: actions/checkout@v1 + - name: cmake configure + run: cmake . + - name: cmake build + run: cmake --build . -j4 linux-build: runs-on: ubuntu-latest steps: @@ -19,7 +26,7 @@ jobs: - name: cmake configure run: cmake . - name: cmake build - run: cmake --build . + run: cmake --build . -j4 - name: network run: sudo apt install net-tools && ifconfig - name: Run Tests From f96fa59a8b8f052257edc672f98befe0e9b2129a Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 15 Feb 2025 22:28:02 -0800 Subject: [PATCH 20/34] Remove mac CI. Was an attempt. --- .github/workflows/ccpp.yml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 45f662d..ea900f7 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,14 +11,6 @@ jobs: run: cmake . - name: cmake build run: cmake --build . -j4 - mac-build: - runs-on: macos-latest - steps: - - uses: actions/checkout@v1 - - name: cmake configure - run: cmake . - - name: cmake build - run: cmake --build . -j4 linux-build: runs-on: ubuntu-latest steps: From 7e8d15f76fcce913a87541d04cb127adbc79a39b Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 22 Feb 2025 01:54:06 -0800 Subject: [PATCH 21/34] Add some additional tests and fix things as necessary Unify argument order for generated message C functions --- include/pubsub/Node.h | 14 +++-- include/pubsub/Serialization.h | 4 +- include/pubsub_cpp/Node.h | 104 +++++++++++++++++--------------- include/pubsub_cpp/Spinners.h | 34 ++++++----- src/Node.c | 22 +++---- src/Publisher.c | 12 ++-- tests/test_cli.cpp | 4 +- tests/test_pubsub_c.cpp | 107 ++++++++++++++++++++++++++++++--- tests/test_pubsub_cpp.cpp | 64 +++++++++++++++++++- tools/generator.cpp | 10 +-- 10 files changed, 269 insertions(+), 106 deletions(-) diff --git a/include/pubsub/Node.h b/include/pubsub/Node.h index 8a572f9..8eae74c 100644 --- a/include/pubsub/Node.h +++ b/include/pubsub/Node.h @@ -162,20 +162,22 @@ struct ps_advertise_req_t uint32_t type_hash;// to see if the type is correct uint32_t group_id;// unique (hopefully) id that indicates which process this node is a part of }; -#pragma pack(pop) -#pragma pack(push) -#pragma pack(1) struct ps_subscribe_req_t { uint8_t id; int32_t addr; uint16_t port; }; -#pragma pack(pop) -#pragma pack(push) -#pragma pack(1) +struct ps_unsubscribe_req_t +{ + uint8_t id; + uint32_t addr; + uint16_t port; + uint32_t stream_id; +}; + struct ps_subscribe_accept_t { uint8_t pid;// packet type identifier diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index 80b43a5..8bf319e 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -78,9 +78,9 @@ extern "C" void ps_msg_ref_free(struct ps_msg_ref_t* msg); struct ps_allocator_t; - typedef struct ps_msg_t(*ps_fn_encode_t)(struct ps_allocator_t* allocator, const void* msg); + typedef struct ps_msg_t(*ps_fn_encode_t)(const void* msg, struct ps_allocator_t* allocator); typedef void*(*ps_fn_decode_t)(const void* data, struct ps_allocator_t* allocator);// allocates the message - typedef void (*ps_fn_free_t)(struct ps_allocator_t* allocator, void* msg);// frees the message + typedef void (*ps_fn_free_t)(void* msg, struct ps_allocator_t* allocator);// frees the message struct ps_message_definition_t { unsigned int hash; diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index bfbff1d..9581be4 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -41,7 +41,7 @@ static std::multimap _subscribers; // ns should not have a leading slash, topic should if it is absolute inline std::string handle_remap(const std::string& topic, const std::string& ns) { - //printf("Handling remap of %s in ns %s\n", topic.c_str(), ns.c_str()); + //printf("Handling remap of %s in ns %s\n", topic.c_str(), ns.c_str()); // we need at least one character if (topic.length() == 0) { @@ -87,10 +87,14 @@ inline std::string handle_remap(const std::string& topic, const std::string& ns) } // okay, we had no remappings, use our namespace - if (ns.length()) - return "/" + ns + "/" + topic; - else - return "/" + topic; + if (ns.length()) + { + return "/" + ns + "/" + topic; + } + else + { + return "/" + topic; + } } // not thread safe @@ -119,7 +123,7 @@ inline void initialize(const char** args, const int argc) // valid names must be all lowercase and only inline std::string validate_name(const std::string& name, bool remove_leading_slashes = false) { - //printf("Validating %s\n", name.c_str()); + //printf("Validating %s\n", name.c_str()); for (size_t i = 0; i < name.length(); i++) { if (name[i] >= 'A' && name[i] <= 'Z') @@ -138,8 +142,8 @@ inline std::string validate_name(const std::string& name, bool remove_leading_sl return name.substr(i); } - else - { + else + { // remove any duplicate slashes we may have std::string out; if (name.length()) @@ -155,7 +159,7 @@ inline std::string validate_name(const std::string& name, bool remove_leading_sl out += name[i]; } return out; - } + } return name; } @@ -211,12 +215,14 @@ class Node std::string getQualifiedName() { - if (namespace_.length()) - return "/" + namespace_ + "/" + real_name_; - else - { - return "/" + real_name_; - } + if (namespace_.length()) + { + return "/" + namespace_ + "/" + real_name_; + } + else + { + return "/" + real_name_; + } } const std::string& getName() @@ -239,10 +245,10 @@ class Node return ps_node_spin(&node_); } - inline void setEventSet(ps_event_set_t* set) - { - event_set_ = set; - } + inline void setEventSet(ps_event_set_t* set) + { + event_set_ = set; + } inline ps_event_set_t* getEventSet() { @@ -311,14 +317,14 @@ class Publisher: public PublisherBase _publisher_mutex.lock(); _publishers.insert(std::pair(remapped_topic_, this)); - // look for any matching subscribers and add them to our list - auto iterpair = _subscribers.equal_range(topic); - for (auto it = iterpair.first; it != iterpair.second; ++it) - { - node.lock_.lock(); - subs_.push_back(it->second); - node.lock_.unlock(); - } + // look for any matching subscribers and add them to our list + auto iterpair = _subscribers.equal_range(topic); + for (auto it = iterpair.first; it != iterpair.second; ++it) + { + node.lock_.lock(); + subs_.push_back(it->second); + node.lock_.unlock(); + } _publisher_mutex.unlock(); } @@ -367,10 +373,8 @@ class Publisher: public PublisherBase // loop through shared subscribers node_->lock_.lock(); // now go through my local subscriber list - for (size_t i = 0; i < subs_.size(); i++) + for (auto& sub: subs_) { - auto sub = subs_[i]; - //printf("Publishing locally with no copy..\n"); auto specific_sub = (Subscriber*)sub; @@ -394,7 +398,8 @@ class Publisher: public PublisherBase void publish(const T& msg) { std::shared_ptr copy; - if (latched_) { + if (latched_) + { copy = std::shared_ptr(new T); *copy = msg; // save for later @@ -402,10 +407,8 @@ class Publisher: public PublisherBase } node_->lock_.lock(); // now go through my local subscriber list - for (size_t i = 0; i < subs_.size(); i++) + for (auto& sub: subs_) { - auto sub = subs_[i]; - //printf("Publishing locally with a copy..\n"); if (!copy) { @@ -481,7 +484,7 @@ class SubscriberBase } } - _subscribers.insert(std::pair(topic, sub)); + _subscribers.insert(std::pair(topic, sub)); _publisher_mutex.unlock(); } @@ -499,21 +502,23 @@ class SubscriberBase // remove me from its list if im there auto pos = std::find(it->second->subs_.begin(), it->second->subs_.end(), sub); if (pos != it->second->subs_.end()) + { it->second->subs_.erase(pos); + } it->second->GetNode()->lock_.unlock(); } } - //remove me from the subscriber list - auto subiterpair = _subscribers.equal_range(topic); - for (auto it = subiterpair.first; it != subiterpair.second; ++it) - { - if (it->second == sub) - { - _subscribers.erase(it); - break; - } - } + //remove me from the subscriber list + auto subiterpair = _subscribers.equal_range(topic); + for (auto it = subiterpair.first; it != subiterpair.second; ++it) + { + if (it->second == sub) + { + _subscribers.erase(it); + break; + } + } _publisher_mutex.unlock(); } @@ -528,8 +533,6 @@ class SubscriberBase virtual bool CallOne() = 0; }; - - template class Subscriber: public SubscriberBase { @@ -545,7 +548,7 @@ class Subscriber: public SubscriberBase public: - Subscriber(Node& node, const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = -1) : cb_(cb), queue_size_(queue_size) + Subscriber(Node& node, const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = -1, int skip = 0) : cb_(cb), queue_size_(queue_size) { node_ = &node; @@ -571,14 +574,13 @@ class Subscriber: public SubscriberBase struct ps_subscriber_options options; ps_subscriber_options_init(&options); - + options.skip = skip; options.cb = cb2; options.cb_data = this; options.allocator = 0; options.ignore_local = true; options.preferred_transport = preferred_transport; - node.lock_.lock(); ps_node_create_subscriber_adv(node.getNode(), remapped_topic_.c_str(), T::GetDefinition(), &subscriber_, &options); node.subscribers_.push_back(this); @@ -631,7 +633,9 @@ class Subscriber: public SubscriberBase node_->lock_.lock(); auto it = std::find(node_->subscribers_.begin(), node_->subscribers_.end(), this); if (it != node_->subscribers_.end()) + { node_->subscribers_.erase(it); + } ps_sub_destroy(&subscriber_); node_->lock_.unlock(); diff --git a/include/pubsub_cpp/Spinners.h b/include/pubsub_cpp/Spinners.h index b07bb2d..bafccd6 100644 --- a/include/pubsub_cpp/Spinners.h +++ b/include/pubsub_cpp/Spinners.h @@ -123,6 +123,7 @@ class BlockingSpinnerWithTimers BlockingSpinnerWithTimers(int num_threads = 1) : running_(false), node_(0) { //ps_event_set_create(&events_); + stop(true); } ~BlockingSpinnerWithTimers() @@ -138,7 +139,7 @@ class BlockingSpinnerWithTimers void setNode(Node& node) { - node_ = &node; + node_ = &node; //list_mutex_.lock(); // build a wait list for all nodes @@ -154,13 +155,12 @@ class BlockingSpinnerWithTimers { while (running_ && ps_okay()) { - //printf("Entering thread\n"); list_mutex_.lock(); if (node_ == 0)//ps_event_set_count(&events_) == 0) { - printf("Waiting for events\n"); + printf("Waiting for events\n"); ps_sleep(10); - continue; + continue; } else { @@ -184,15 +184,15 @@ class BlockingSpinnerWithTimers // this line is necessary anyways, but happens to work around the above bug timeout = std::min(timeout, 1000000);// make sure we dont block too long - //printf("setting timeout to %i\n", timeout); + //printf("setting timeout to %i\n", timeout); } list_mutex_.unlock(); - //ps_node_wait(node_->getNode(), 0); + //ps_node_wait(node_->getNode(), 0); // todo allow finer grained waits //if (timeout > 2) { // allows for fine grained waits - ps_event_set_set_timer(&node_->getNode()->events, timeout);// in us + ps_event_set_set_timer(&node_->getNode()->events, timeout);// in us ps_node_wait(node_->getNode(), 1000); //ps_event_set_wait(&events_, timeout); } @@ -205,7 +205,7 @@ class BlockingSpinnerWithTimers Time now = Time::now(); if (now >= timer.next_trigger) { - //printf("Calling timer\n"); + //printf("Calling timer\n"); timer.next_trigger = timer.next_trigger + timer.period; timer.fn(); } @@ -213,7 +213,7 @@ class BlockingSpinnerWithTimers // check all nodes //for (auto node : nodes_) - if (node_) + if (node_) { node_->lock_.lock(); if (ps_node_spin(node_->getNode()) || node_->marked()) @@ -249,7 +249,12 @@ class BlockingSpinnerWithTimers void wait() { - if (!running_) + thread_.join(); + } + + void run() + { + if (!running_) { start(); } @@ -265,8 +270,10 @@ class BlockingSpinnerWithTimers } running_ = false; - if (join && thread_.joinable()) - thread_.join();// wait for it to stop + if (join && thread_.joinable()) + { + thread_.join();// wait for it to stop + } } }; @@ -301,7 +308,7 @@ class Spinner int res = 0; if (res = ps_node_spin(node->getNode()) || node->marked()) { - printf("Received %i messages\n", res); + printf("Received %i messages\n", res); // we got a message, now call a subscriber // todo how to make this not scale with subscriber count... for (size_t i = 0; i < node->subscribers_.size(); i++) @@ -311,7 +318,6 @@ class Spinner } } - node->lock_.unlock(); } list_mutex_.unlock(); diff --git a/src/Node.c b/src/Node.c index ffdd1c9..f404540 100644 --- a/src/Node.c +++ b/src/Node.c @@ -768,9 +768,10 @@ int ps_node_spin(struct ps_node_t* node) socklen_t fromLength = sizeof(from); int received_bytes = recvfrom(node->socket, (char*)data, size, 0, (struct sockaddr*)&from, &fromLength); - if (received_bytes <= 0) + { break; + } #ifdef PUBSUB_VERBOSE //printf("got transport packet\n"); @@ -974,9 +975,10 @@ int ps_node_spin(struct ps_node_t* node) socklen_t fromLength = sizeof(from); int received_bytes = recvfrom(node->mc_socket, (char*)data, size, 0, (struct sockaddr*)&from, &fromLength); - if (received_bytes <= 0) + { break; + } //printf("Got discovery msg \n"); @@ -1147,7 +1149,9 @@ int ps_node_spin(struct ps_node_t* node) //printf("recommended transport: %i\n", recommended_transport); if (preferred_transport != 0) + { preferred_transport = (1 << (preferred_transport-1)); + } // first match udp if its what we want or all that is offered if (preferred_transport == PS_TRANSPORT_UDP || p->transports == PS_TRANSPORT_UDP) { @@ -1225,13 +1229,9 @@ int ps_node_spin(struct ps_node_t* node) else if (data[0] == PS_DISCOVERY_PROTOCOL_UNSUBSCRIBE) { //printf("Got unsubscribe request\n"); + struct ps_unsubscribe_req_t* msg = (struct ps_unsubscribe_req_t*)data; - int* addr = (int*)&data[1]; - unsigned short* port = (unsigned short*)&data[5]; - - unsigned int* stream_id = (unsigned int*)&data[7]; - - char* topic = (char*)&data[11]; + char* topic = (char*)&data[sizeof(struct ps_unsubscribe_req_t)]; //check if we have a sub matching that topic struct ps_pub_t* pub = 0; @@ -1253,9 +1253,9 @@ int ps_node_spin(struct ps_node_t* node) // remove the client struct ps_client_t client; - client.endpoint.address = *addr; - client.endpoint.port = *port; - client.stream_id = *stream_id; + client.endpoint.address = msg->addr; + client.endpoint.port = msg->port; + client.stream_id = msg->stream_id; ps_pub_remove_client(pub, &client); } else if (data[0] == PS_DISCOVERY_PROTOCOL_QUERY_ALL) diff --git a/src/Publisher.c b/src/Publisher.c index 11cc6f8..d47460d 100644 --- a/src/Publisher.c +++ b/src/Publisher.c @@ -10,10 +10,10 @@ #include -void ps_pub_publish_client(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_ref_t* msg) +static void ps_pub_publish_client(struct ps_pub_t* pub, struct ps_client_t* client, struct ps_msg_ref_t* msg, bool force_publish) { // Skip messages if desired by the client - if (client->modulo > 0) + if (client->modulo > 0 && force_publish == false) { if (pub->sequence_number % client->modulo != 0) { @@ -72,8 +72,8 @@ bool ps_pub_add_client(struct ps_pub_t* pub, const struct ps_client_t* client) // If we are latched, send the new client our last message if (pub->last_message && pub->latched) { - //printf("publishing latched\n"); - ps_pub_publish_client(pub, &pub->clients[pub->num_clients - 1], pub->last_message); + //printf("publishing latched\n"); + ps_pub_publish_client(pub, &pub->clients[pub->num_clients - 1], pub->last_message, true); } return true; } @@ -140,7 +140,7 @@ void ps_pub_publish_ez(struct ps_pub_t* pub, void* msg) { if (pub->num_clients > 0 || pub->latched) { - struct ps_msg_t data = pub->message_definition->encode(0, msg); + struct ps_msg_t data = pub->message_definition->encode(msg, 0); ps_pub_publish(pub, &data); } @@ -174,7 +174,7 @@ void ps_pub_publish(struct ps_pub_t* pub, struct ps_msg_t* msg) { struct ps_client_t* client = &pub->clients[i]; - ps_pub_publish_client(pub, client, ref); + ps_pub_publish_client(pub, client, ref, false); } if (pub->latched) diff --git a/tests/test_cli.cpp b/tests/test_cli.cpp index 325da47..36b0047 100644 --- a/tests/test_cli.cpp +++ b/tests/test_cli.cpp @@ -100,7 +100,7 @@ TEST(test_cli_pub_latched, []() { spinner.stop(); }, 10); - spinner.wait(); + spinner.run(); EXPECT(got_message); run = false; @@ -126,7 +126,7 @@ TEST(test_cli_pub, []() { spinner.stop(); }, 10); - spinner.wait(); + spinner.run(); EXPECT(got_message); run = false; diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index ba465ac..849aaac 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -52,8 +52,7 @@ TEST(test_publish_subscribe_generic, []() { auto data = (struct pubsub__String*)pubsub__String_decode(message, &ps_default_allocator); printf("Got message: %s\n", data->value); EXPECT(strcmp(data->value, rmsg.value) == 0); - free(data->value); - free(data); + pubsub__String_free(data, &ps_default_allocator); free(message); }; @@ -100,8 +99,7 @@ void latch_test_cb(bool broadcast, bool tcp) { auto data = (struct pubsub__String*)message; EXPECT(strcmp(data->value, rmsg.value) == 0); - free(data->value); - free(data);//todo use allocator free + pubsub__String_free(data, &ps_default_allocator); got_message = true; }; ps_node_create_subscriber_adv(&node, "/data", &pubsub__String_def, &string_sub, &options); @@ -159,7 +157,6 @@ TEST(test_publish_subscribe_large, []() { struct ps_subscriber_options options; ps_subscriber_options_init(&options); - //options.skip = skip; options.allocator = 0; options.ignore_local = false; @@ -174,8 +171,7 @@ TEST(test_publish_subscribe_large, []() { auto data = (struct pubsub__PointCloud*)pubsub__PointCloud_decode(message, &ps_default_allocator); printf("Decoded message\n"); EXPECT(data->num_points == rmsg.num_points); - free(data->data); - free(data); + pubsub__PointCloud_free(data, &ps_default_allocator); free(message); }; ps_node_create_subscriber_adv(&node, "/data", 0, &string_sub, &options); @@ -193,4 +189,101 @@ TEST(test_publish_subscribe_large, []() { ps_node_destroy(&node); }); +TEST(test_publish_subscribe_latched_skip, []() { + // test that we still get the latched message even if we want to skip messages + struct ps_node_t node; + ps_node_init(&node, "test_node", "", false); + + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, &node); + ps_node_add_transport(&node, &tcp_transport); + + struct ps_pub_t string_pub; + ps_node_create_publisher(&node, "/data", &pubsub__String_def, &string_pub, true); + + // come up with the latched topic + static struct pubsub__String rmsg; + rmsg.value = "Hello"; + ps_pub_publish_ez(&string_pub, &rmsg); + + static bool got_message = false; + got_message = false; + + struct ps_sub_t string_sub; + struct ps_subscriber_options options; + ps_subscriber_options_init(&options); + options.skip = 100; + options.cb = [](void* message, unsigned int size, void* cb_data, const struct ps_msg_info_t* info) + { + auto data = (struct pubsub__String*)message; + EXPECT(strcmp(data->value, rmsg.value) == 0); + pubsub__String_free(data, &ps_default_allocator); + got_message = true; + }; + ps_node_create_subscriber_adv(&node, "/data", &pubsub__String_def, &string_sub, &options); + + // now spin and wait for us to get the published message + while (ps_okay() && !got_message) + { + ps_node_spin(&node); + ps_sleep(1); + } + + EXPECT(got_message); + + ps_node_destroy(&node); +}); + +TEST(test_publish_subscribe_skip, []() { + // test that skip works correctly + struct ps_node_t node; + ps_node_init(&node, "test_node", "", false); + + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, &node); + ps_node_add_transport(&node, &tcp_transport); + + struct ps_pub_t string_pub; + ps_node_create_publisher(&node, "/data", &pubsub__String_def, &string_pub, true); + + // come up with the latched topic + static struct pubsub__String rmsg; + rmsg.value = "Hello"; + ps_pub_publish_ez(&string_pub, &rmsg); + + struct ps_sub_t string_sub; + struct ps_subscriber_options options; + ps_subscriber_options_init(&options); + options.skip = 10; + static int received = 0; + options.cb = [](void* message, unsigned int size, void* cb_data, const struct ps_msg_info_t* info) + { + auto data = (struct pubsub__String*)message; + EXPECT(strcmp(data->value, rmsg.value) == 0); + pubsub__String_free(data, &ps_default_allocator); + received++; + }; + ps_node_create_subscriber_adv(&node, "/data", &pubsub__String_def, &string_sub, &options); + + // first spin and wait for connection + while (ps_okay() && ps_pub_get_subscriber_count(&string_pub) == 0) + { + ps_node_spin(&node); + ps_sleep(1); + } + + // now spin and publish + for (int i = 0; i < 100; i++) + { + ps_node_spin(&node); + ps_pub_publish_ez(&string_pub, &rmsg); + ps_sleep(1); + } + + // finally count the number of messages + EXPECT(received == 10); + + ps_node_destroy(&node); +}); + CREATE_MAIN_ENTRY_POINT(); diff --git a/tests/test_pubsub_cpp.cpp b/tests/test_pubsub_cpp.cpp index 95f0703..a6d1432 100644 --- a/tests/test_pubsub_cpp.cpp +++ b/tests/test_pubsub_cpp.cpp @@ -15,7 +15,6 @@ TEST(test_publish_subscribe_latched_cpp, []() { pubsub::msg::String omsg; omsg.value = "Hello"; string_pub.publish(omsg); - pubsub::BlockingSpinnerWithTimers spinner; spinner.setNode(node); @@ -28,7 +27,66 @@ TEST(test_publish_subscribe_latched_cpp, []() { spinner.stop(); }, 10); - spinner.wait(); + spinner.run(); + EXPECT(got_message); +}); + +TEST(test_publish_subscribe_zero_copy, []() { + // test that data gets passed through without copying within a single node + pubsub::Node node("simple_publisher"); + + pubsub::Publisher string_pub(node, "/data", true); + + pubsub::msg::StringSharedPtr omsg(new pubsub::msg::String()); + omsg->value = "Hello"; + string_pub.publish(omsg); + + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node); + + bool got_message = false; + pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { + printf("Got message %s in sub1\n", msg->value); + EXPECT(strcmp(omsg->value, msg->value) == 0); + got_message = true; + EXPECT(msg.get() == omsg.get()); + spinner.stop(); + }, 10); + + spinner.run(); + EXPECT(got_message); +}); + +TEST(test_publish_subscribe_nodelets, []() { + // test that data gets passed through without copying between multiple nodes + pubsub::Node nodep("simple_publisher"); + pubsub::Node nodes("simple_subscriber"); + + pubsub::Publisher string_pub(nodep, "/data", true); + + pubsub::msg::StringSharedPtr omsg(new pubsub::msg::String()); + omsg->value = "Hello"; + string_pub.publish(omsg); + + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(nodep); + + pubsub::BlockingSpinnerWithTimers spinner2; + spinner2.setNode(nodes); + + bool got_message = false; + pubsub::Subscriber subscriber(nodes, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { + printf("Got message %s in sub1\n", msg->value); + EXPECT(strcmp(omsg->value, msg->value) == 0); + got_message = true; + EXPECT(msg.get() == omsg.get()); + spinner2.stop(); + spinner.stop(); + }, 10); + + spinner2.start(); + spinner.run(); + spinner2.wait(); EXPECT(got_message); }); @@ -98,7 +156,7 @@ TEST(test_publish_subscribe_cpp, []() { string_pub.publish(omsg); }); - spinner.wait(); + spinner.run(); EXPECT(got_message); }); diff --git a/tools/generator.cpp b/tools/generator.cpp index 6af4674..01f7251 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -544,7 +544,7 @@ std::string generate(const char* definition, const char* name) output += "}\n\n"; // now for encode - output += "static struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* msg)\n{\n"; + output += "static struct ps_msg_t " + type_name + "_encode(const void* msg, struct ps_allocator_t* allocator)\n{\n"; output += " int len = sizeof(struct " + type_name + ");\n"; output += " struct ps_msg_t omsg;\n"; output += " ps_msg_alloc(len, allocator, &omsg);\n"; @@ -552,7 +552,7 @@ std::string generate(const char* definition, const char* name) output += " return omsg;\n}\n\n"; // finally free todo use allocator - output += "static void " + type_name + "_free(struct ps_allocator_t* allocator, void* msg)\n{\n"; + output += "static void " + type_name + "_free(void* msg, struct ps_allocator_t* allocator)\n{\n"; output += " allocator->free(msg, allocator->context);\n"; output += "}\n\n"; } @@ -633,7 +633,7 @@ std::string generate(const char* definition, const char* name) output += "}\n\n"; //typedef ps_msg_t(*ps_fn_encode_t)(ps_allocator_t* allocator, const void* msg); - output += "static struct ps_msg_t " + type_name + "_encode(struct ps_allocator_t* allocator, const void* data)\n{\n"; + output += "static struct ps_msg_t " + type_name + "_encode(const void* data, struct ps_allocator_t* allocator)\n{\n"; output += " const struct " + type_name + "* msg = (const struct " + type_name + "*)data;\n"; output += " int len = sizeof(struct " + type_name + ");\n"; output += " // calculate the encoded length of the message\n"; @@ -737,7 +737,7 @@ std::string generate(const char* definition, const char* name) output += "}\n"; // finally free - output += "static void " + type_name + "_free(struct ps_allocator_t* allocator, void* data)\n{\n"; + output += "static void " + type_name + "_free(void* data, struct ps_allocator_t* allocator)\n{\n"; output += " struct " + type_name + "* msg = (struct " + type_name + "*)data;\n"; for (size_t i = 0; i < fields.size(); i++) { @@ -862,7 +862,7 @@ std::string generate(const char* definition, const char* name) output += " static const ps_message_definition_t* GetDefinition()\n {\n"; output += " return &" + type_name + "_def;\n }\n\n"; output += " ps_msg_t Encode() const\n {\n"; - output += " return " + ns + "__" + raw_name + "_encode(&ps_default_allocator, this);\n }\n\n"; + output += " return " + ns + "__" + raw_name + "_encode(this, &ps_default_allocator);\n }\n\n"; output += " static " + raw_name + "* Decode(const void* data)\n {\n"; output += " return (" + raw_name + "*)" + ns + "__" + raw_name + "_decode(data, &ps_default_allocator);\n }\n";// + ns + "__" + raw_name + "_encode(0, this);\n }\n"; output += "};\n"; From 2a7f1ad1acc1e16f4eba962b28ec82d3d0581dde Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sat, 22 Feb 2025 12:32:45 -0800 Subject: [PATCH 22/34] Add test for queue size and correct subscriber count for local subscribers --- include/pubsub_cpp/Node.h | 3 +- msg/Int.msg | 1 + tests/test_pubsub_cpp.cpp | 66 +++++++++++++++++++++++---------------- tools/CMakeLists.txt | 1 + 4 files changed, 42 insertions(+), 29 deletions(-) create mode 100644 msg/Int.msg diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index 9581be4..fd28b83 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -439,7 +439,7 @@ class Publisher: public PublisherBase unsigned int getNumSubscribers() { - return ps_pub_get_subscriber_count(&publisher_); + return ps_pub_get_subscriber_count(&publisher_) + subs_.size(); } void addCustomEndpoint(const int ip_addr, const short port, const unsigned int stream_id) @@ -475,7 +475,6 @@ class SubscriberBase // if its latched, get the message from it if (it->second->latched_) { - // hmm, this should just queue not call cb(it->second); } // add me to its sub list diff --git a/msg/Int.msg b/msg/Int.msg new file mode 100644 index 0000000..18ba850 --- /dev/null +++ b/msg/Int.msg @@ -0,0 +1 @@ +int64 value diff --git a/tests/test_pubsub_cpp.cpp b/tests/test_pubsub_cpp.cpp index a6d1432..fe25edf 100644 --- a/tests/test_pubsub_cpp.cpp +++ b/tests/test_pubsub_cpp.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include "mini_mock.hpp" @@ -25,6 +26,9 @@ TEST(test_publish_subscribe_latched_cpp, []() { EXPECT(strcmp(omsg.value, msg->value) == 0); got_message = true; spinner.stop(); + + // make sure subscriber count is correct + EXPECT(string_pub.getNumSubscribers() == 1); }, 10); spinner.run(); @@ -51,12 +55,47 @@ TEST(test_publish_subscribe_zero_copy, []() { got_message = true; EXPECT(msg.get() == omsg.get()); spinner.stop(); + + // make sure subscriber count is correct + EXPECT(string_pub.getNumSubscribers() == 1); }, 10); spinner.run(); EXPECT(got_message); }); +TEST(test_publish_subscribe_queue_behavior, []() { + // test that the queue drops messages as expected and we only get the newest + pubsub::Node node("simple_publisher"); + + pubsub::Publisher int_pub(node, "/data"); + + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node); + + std::vector received; + pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::IntSharedPtr& msg) { + printf("Got message %i in sub1\n", msg->value); + received.push_back(msg->value); + spinner.stop(); + }, 10); + + for (int i = 0; i < 100; i++) + { + pubsub::msg::Int omsg; + omsg.value = i; + int_pub.publish(omsg); + } + + // spin after publishing so the queue fills up + spinner.run(); + EXPECT(received.size() == 10); + for (int i = 0; i < 10; i++) + { + EXPECT(received[i] == 90 + i); + } +}); + TEST(test_publish_subscribe_nodelets, []() { // test that data gets passed through without copying between multiple nodes pubsub::Node nodep("simple_publisher"); @@ -104,33 +143,6 @@ TEST(test_publisher_subscriber_close_cpp, []() { sub.close(); }); -/*TEST(test_publish_subscribe_latched_cpp, []() { - // test that latched topics make it through the local message passing between nodes - pubsub::Node node("simple_publisher"); - - pubsub::Publisher string_pub(node, "/data", true); - - pubsub::msg::String omsg; - omsg.value = "Hello"; - string_pub.publish(omsg); - - - pubsub::BlockingSpinnerWithTimers spinner; - spinner.setNode(node); - - bool got_message = false; - pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp(omsg.value, msg->value) == 0); - got_message = true; - spinner.stop(); - }, 10); - - spinner.wait(); - EXPECT(got_message); -});*/ - - TEST(test_publish_subscribe_cpp, []() { // test that normal messages make it through message passing pubsub::Node node("simple_publisher"); diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index fea1c57..30c8e27 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -69,6 +69,7 @@ generate_messages(FILES Log.msg Path2D.msg Odom.msg + Int.msg ) From 0f3df952dce133c5c66d098489c753d5a83ccab2 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 23 Feb 2025 23:45:11 -0800 Subject: [PATCH 23/34] Support arrays in structs Support fixed length strings in messages --- examples/simple_logger.cpp | 3 +- examples/simple_pub.cpp | 2 +- examples/simple_sub.c | 2 +- examples/simple_sub.cpp | 2 +- include/pubsub/Serialization.h | 3 +- include/pubsub_cpp/array_string.h | 179 ++++++++++++++++++++++++++++++ src/Serialization.c | 48 +++++--- tests/mini_mock.hpp | 23 ++++ tests/test_cli.cpp | 8 +- tests/test_pubsub_cpp.cpp | 21 ++-- tests/test_serialization.cpp | 49 ++++++-- tools/generator.cpp | 101 ++++++++++++++--- tools/throughput_test.cpp | 3 +- 13 files changed, 387 insertions(+), 57 deletions(-) create mode 100644 include/pubsub_cpp/array_string.h diff --git a/examples/simple_logger.cpp b/examples/simple_logger.cpp index 5d4ad89..6ac4b61 100644 --- a/examples/simple_logger.cpp +++ b/examples/simple_logger.cpp @@ -32,8 +32,7 @@ int main() // okay, since we are publishing with shared pointer we actually need to allocate the string properly auto shared = pubsub::msg::StringSharedPtr(new pubsub::msg::String); - shared->value = new char[strlen(msg.value) + 1]; - strcpy(shared->value, msg.value); + shared->value = msg.value; string_pub.publish(shared); msg.value = 0;// so it doesnt get freed by the destructor since we allocated it ourself diff --git a/examples/simple_pub.cpp b/examples/simple_pub.cpp index 0f0c750..0e8a538 100644 --- a/examples/simple_pub.cpp +++ b/examples/simple_pub.cpp @@ -44,7 +44,7 @@ int main() }); // Wait for the spinner to exit (on control-c) - spinner.wait(); + spinner.run(); return 0; } diff --git a/examples/simple_sub.c b/examples/simple_sub.c index 0d52f2c..03dd488 100644 --- a/examples/simple_sub.c +++ b/examples/simple_sub.c @@ -15,7 +15,7 @@ void callback(void* message, unsigned int size, void* cbdata, const struct ps_ms // user is responsible for freeing the message and its arrays struct pubsub__String* data = (struct pubsub__String*)message; printf("Got message: %s\n", data->value); - pubsub__String_free(string_sub.allocator, data); + pubsub__String_free(data, string_sub.allocator); } int main() diff --git a/examples/simple_sub.cpp b/examples/simple_sub.cpp index 4c0ff35..7e3fd26 100644 --- a/examples/simple_sub.cpp +++ b/examples/simple_sub.cpp @@ -15,7 +15,7 @@ int main() // Create the subscriber, the provided callback will be called each time a message comes in pubsub::Subscriber subscriber(node, "data"/*topic name*/, [](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s\n", msg->value); + printf("Got message %s\n", msg->value.c_str()); }, 10/*maximum queue size, after this many messages build up the oldest will get dropped*/); // Create the "spinner" which executes callbacks and timers in a background thread diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index 8bf319e..d0263ef 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -31,8 +31,9 @@ extern "C" FT_Float32 = 9, FT_Float64 = 10, FT_MaxFloat,// all floats and ints are less than this, not present in messages - FT_String, + FT_String,// null terminated dynamic length string FT_Struct,//indicates the number of fields following contained in it + FT_ArrayString// null terminated fixed length string }; typedef enum ps_field_types ps_field_types; diff --git a/include/pubsub_cpp/array_string.h b/include/pubsub_cpp/array_string.h new file mode 100644 index 0000000..853578b --- /dev/null +++ b/include/pubsub_cpp/array_string.h @@ -0,0 +1,179 @@ + +#pragma once + +#include +#include +#include + +#pragma pack(push, 1) +// Wrapper for a fixed size array for using it like a string +template +class FixedString +{ + char data_[string_length]; +public: + + FixedString() + { + data_[0] = 0; + } + + operator const char*() + { + return data_; + } + + void operator=(const std::string& string) + { + if (string.length() >= string_length) + { + throw std::runtime_error("Too big."); + } + strcpy(data_, string.c_str()); + } + + void operator=(const char* string) + { + // todo make this more efficient? + if (strlen(string) >= string_length) + { + throw std::runtime_error("Too big."); + } + strncpy(data_, string, string_length - 1); + } + + bool operator==(const char* other) + { + return strcmp(other, data_) == 0; + } + + bool operator==(const std::string& other) + { + return strcmp(other.c_str(), data_) == 0; + } + + char* data() const + { + return data_; + } + + const char* c_str() const + { + return data_; + } + + int max_size() const + { + return string_length; + } + + int length() const + { + return strlen(data_); + } +}; + +//would have to require the allocator in this class as part of the template +// Wrapper for a C string which makes it easier to use and handles freeing +// todo how to handle allocators? +class CString +{ + char* data_; +public: + + CString() + { + data_ = 0; + } + + CString(const CString& other) + { + data_ = 0; + if (other.data_) + { + data_ = strdup(other.data_); + } + } + + ~CString() + { + if (data_) + { + free(data_); + } + } + + void operator=(const std::string& string) + { + if (data_) + { + free(data_); + } + data_ = (char*)malloc(string.length()+1); + strcpy(data_, string.c_str()); + } + + void operator=(const char* string) + { + if (data_) + { + free(data_); + } + data_ = (char*)malloc(strlen(string)+1); + strcpy(data_, string); + } + + void operator=(const CString& other) + { + if (data_) + { + free(data_); + } + if (other.data_) + { + data_ = strdup(other.data_); + } + } + + bool operator==(const char* other) + { + if (data_ == 0) + { + return strcmp(other, "") == 0; + } + return strcmp(other, data_) == 0; + } + + bool operator==(const std::string& other) + { + if (data_ == 0) + { + return strcmp(other.c_str(), "") == 0; + } + return strcmp(other.c_str(), data_) == 0; + } + + bool operator==(const CString& other) + { + if (data_ == 0) + { + return strcmp(other.c_str(), "") == 0; + } + return strcmp(other.c_str(), data_) == 0; + } + + char* data() const + { + return data_; + } + + const char* c_str() const + { + if (data_ == 0) + { + return ""; + } + return data_; + } +}; +#pragma pack(pop) diff --git a/src/Serialization.c b/src/Serialization.c index 3bc9f37..ff31332 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -308,11 +308,11 @@ const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const s return position; } -static uint64_t print_field(int type, const char** ptr) +static uint64_t print_field(const struct ps_msg_field_t* field, const char** ptr) { uint64_t value = 0; // non dynamic types - switch (type) + switch (field->type) { case FT_Int8: printf("%i", (int)*(int8_t*)*ptr); @@ -362,6 +362,10 @@ static uint64_t print_field(int type, const char** ptr) printf("%lf", *(double*)*ptr); *ptr += 8; break; + case FT_ArrayString: + printf("\"%s\"", (char*)*ptr); + *ptr += field->content_length; + break; default: printf("ERROR: unhandled field type when parsing....\n"); } @@ -372,9 +376,10 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ { struct ps_deserialize_iterator iter = ps_deserialize_start(data, definition); const struct ps_msg_field_t* field; uint32_t length; const char* ptr; - int it = 0; + int it = -1; while (ptr = ps_deserialize_iterate(&iter, &field, &length)) { + it++; if (field_name && strcmp(field_name, field->name) != 0) { continue; @@ -384,7 +389,7 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ // strings are already null terminated if (field->length == 1) { - printf("%s: %s\n", field->name, ptr); + printf("%s: \"%s\"\n", field->name, ptr); } else { @@ -427,9 +432,29 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ printf(" { "); for (int f = 0; f < iter.struct_num_fields; f++) { - printf("%s: ", (field+1+f)->name); - print_field((field+1+f)->type, &ptr); - printf(" "); + const struct ps_msg_field_t* sf = (field+1+f); + printf("%s: ", sf->name); + if (sf->length > 1) + { + printf("["); + for (int i = 0; i < sf->length; i++) + { + print_field(sf, &ptr); + if (i != sf->length - 1) + { + printf(", "); + } + } + printf("]"); + } + else + { + print_field(sf, &ptr); + } + if (f != (iter.struct_num_fields - 1)) + { + printf(", "); + } } printf(" }"); } @@ -468,7 +493,7 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ for (unsigned int i = 0; i < length; i++) { uint64_t value = 0; - value = print_field(field->type, &ptr); + value = print_field(field, &ptr); if (field->flags > 0) { @@ -514,7 +539,6 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ } } } - it++; } } @@ -527,8 +551,6 @@ const char* TypeToString(ps_field_types type) { switch (type) { - // declarations - // . . . case FT_Int8: return "int8"; case FT_Int16: @@ -550,11 +572,11 @@ const char* TypeToString(ps_field_types type) case FT_Float64: return "double"; case FT_String: - // statements executed if the expression equals the - // value of this constant_expression return "string"; case FT_Struct: return "struct"; + case FT_ArrayString: + return "astring"; default: return "Unknown Type"; } diff --git a/tests/mini_mock.hpp b/tests/mini_mock.hpp index 28c34f4..79c9ba5 100644 --- a/tests/mini_mock.hpp +++ b/tests/mini_mock.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include // Useful console colors @@ -83,6 +84,28 @@ static int mini_mock_failed_conditions_count = 0; } \ } +// If an exception matching thie provided message is thrown +// - an automatic message will be printed (with file name and line number) +// - the test continues +// - the test will fail at the end +void EXPECT_THROWS(std::function function, const std::string& message) +{ + bool threw = false; + try + { + function(); + } + catch (std::exception& err) + { + threw = true; + if (message.length()) + { + EXPECT(std::string(err.what()) == message); + } + } + EXPECT(threw); +} + // If condition is false : // - the given message will be printed // - the test fails and stops immediately diff --git a/tests/test_cli.cpp b/tests/test_cli.cpp index 36b0047..4738868 100644 --- a/tests/test_cli.cpp +++ b/tests/test_cli.cpp @@ -94,8 +94,8 @@ TEST(test_cli_pub_latched, []() { bool got_message = false; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp("hello", msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(msg->value == "hello"); got_message = true; spinner.stop(); }, 10); @@ -120,8 +120,8 @@ TEST(test_cli_pub, []() { bool got_message = false; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp("hello", msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(msg->value == "hello"); got_message = true; spinner.stop(); }, 10); diff --git a/tests/test_pubsub_cpp.cpp b/tests/test_pubsub_cpp.cpp index fe25edf..c1ef742 100644 --- a/tests/test_pubsub_cpp.cpp +++ b/tests/test_pubsub_cpp.cpp @@ -22,8 +22,8 @@ TEST(test_publish_subscribe_latched_cpp, []() { bool got_message = false; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp(omsg.value, msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(omsg.value == msg->value); got_message = true; spinner.stop(); @@ -50,8 +50,8 @@ TEST(test_publish_subscribe_zero_copy, []() { bool got_message = false; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp(omsg->value, msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(omsg->value == msg->value); got_message = true; EXPECT(msg.get() == omsg.get()); spinner.stop(); @@ -75,7 +75,7 @@ TEST(test_publish_subscribe_queue_behavior, []() { std::vector received; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::IntSharedPtr& msg) { - printf("Got message %i in sub1\n", msg->value); + printf("Got message %li in sub1\n", msg->value); received.push_back(msg->value); spinner.stop(); }, 10); @@ -115,8 +115,8 @@ TEST(test_publish_subscribe_nodelets, []() { bool got_message = false; pubsub::Subscriber subscriber(nodes, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp(omsg->value, msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(omsg->value == msg->value); got_message = true; EXPECT(msg.get() == omsg.get()); spinner2.stop(); @@ -128,7 +128,8 @@ TEST(test_publish_subscribe_nodelets, []() { spinner2.wait(); EXPECT(got_message); }); - +//okay, things to add, fixed length strings so we can have strings in structs +//should also enhance bindings for strings, I think C++ strings have memory leaks // Make sure close works on publishers/subscribers and doesnt result in them getting closed multiple times TEST(test_publisher_subscriber_close_cpp, []() { pubsub::Node node("simple_publisher"); @@ -157,8 +158,8 @@ TEST(test_publish_subscribe_cpp, []() { bool got_message = false; pubsub::Subscriber subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) { - printf("Got message %s in sub1\n", msg->value); - EXPECT(strcmp(omsg.value, msg->value) == 0); + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(omsg.value == msg->value); spinner.stop(); got_message = true; }, 10); diff --git a/tests/test_serialization.cpp b/tests/test_serialization.cpp index 058323a..5220d11 100644 --- a/tests/test_serialization.cpp +++ b/tests/test_serialization.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "mini_mock.hpp" @@ -25,8 +26,46 @@ TEST(test_joy_serialization, []() { free(out); }); -void test2() -{ +TEST(test_string_cpp, []() { + std::string value = "hi"; + // test C++ strings + { + pubsub::msg::String msg; + EXPECT(msg.value.data() == 0) + EXPECT(msg.value == ""); + msg.value = "apples"; + EXPECT(msg.value == "apples"); + EXPECT(msg.value == std::string("apples")); + EXPECT(strcmp(msg.value.c_str(), "apples") == 0); + msg.value = value; + EXPECT(msg.value == value); + EXPECT(msg.value == value.c_str()); + EXPECT(strcmp(msg.value.c_str(), value.c_str()) == 0); + } +}); + +TEST(test_fixed_string_cpp, []() { + // test C++ fixed strings + { + FixedString<5> string; + string = "hi"; + + EXPECT(string == "hi"); + EXPECT(string == std::string("hi")); + EXPECT(strcmp(string.c_str(), "hi") == 0); + EXPECT(sizeof(string) == 5); + } + + // test what happens when you assign too much + { + FixedString<5> string; + EXPECT_THROWS([&](){ + string = "hello paul"; + }, "Too big."); + } +}); + +TEST(test_costmap_c_cpp, []() { // verify that the C type matches the C++ one memory wise pubsub::msg::Costmap msg; msg.width = 100; @@ -48,10 +87,6 @@ void test2() if (msg.data[i] != cmsg->data[i]) EXPECT(false); } -} - -TEST(test_costmap_c_cpp, []() { - test2(); }); TEST(test_costmap_serialization, []() { @@ -82,7 +117,7 @@ TEST(test_costmap_serialization, []() { if (out->data[i] != msg.data[i]) EXPECT(false); } - delete out; + delete out; }); TEST(test_path2d_serialization, []() { diff --git a/tools/generator.cpp b/tools/generator.cpp index 01f7251..86ddfe2 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -30,13 +30,14 @@ struct enumeration int field_num; }; +struct field; struct Type { std::string name; std::string base_type; std::string type_enum; // if zero size, a basic type - std::vector> fields; + std::vector fields; }; // stupid hack @@ -49,6 +50,8 @@ struct field int array_size; + int string_size; + std::string flag; uint32_t line_number; @@ -224,6 +227,7 @@ std::string generate(const char* definition, const char* name) types["int8"] = new Type{"int8", "int8_t", "FT_Int8", {}}; types["float"] = new Type{"float", "float", "FT_Float32", {}}; types["double"] = new Type{"double", "double", "FT_Float64", {}}; + types["astring"] = new Type{"astring", "char", "FT_ArrayString", {}}; // also generate the hash while we are at it uint32_t hash = 0; @@ -297,6 +301,20 @@ std::string generate(const char* definition, const char* name) current_struct->type_enum = "FT_Struct"; continue; } + + int string_size = 0; + if (type.find(':') != -1) + { + int index = type.find(':'); + string_size = std::stoi(type.substr(index + 1)); + type = type.substr(0, index); + + if (type.find("astring") == -1) + { + printf("%s:%i ERROR: The element count syntax can only be used with astrings'.\n", _current_file.c_str(), line_number); + throw 7; + } + } // lookup the type Type* real_type = 0; @@ -314,11 +332,11 @@ std::string generate(const char* definition, const char* name) if (current_struct) { // this belongs in the struct - current_struct->fields.push_back({real_type, name}); + current_struct->fields.push_back(new field{ name, real_type, size, string_size, "", line_number }); continue; } // also fill in array size - fields.push_back({ name, real_type, size, "", line_number }); + fields.push_back({ name, real_type, size, string_size, "", line_number }); } // a line with flags maybe? else if (words.size() == 3 && !has_equal) @@ -353,8 +371,15 @@ std::string generate(const char* definition, const char* name) throw 7; } + if (current_struct) + { + // this belongs in the struct + current_struct->fields.push_back(new field{ name, real_type, size, 0, "", line_number }); + continue; + } + // also fill in array size - fields.push_back({ name, real_type, size, flag, line_number}); + fields.push_back({ name, real_type, size, 0, flag, line_number}); } else { @@ -367,7 +392,8 @@ std::string generate(const char* definition, const char* name) std::string name = strip_whitespace(equals[0]); std::string value = strip_whitespace(equals[1]); //printf("it was an enum: %s=%s\n", equals[0].c_str(), equals[1].c_str()); - enumerations.push_back({ name, value, (int)fields.size()}); + + enumerations.push_back({ name, value, fields.size() }); } else { @@ -378,7 +404,7 @@ std::string generate(const char* definition, const char* name) } std::string raw_name = split(name, '_').back(); - std::string ns = std::string(name).substr(0, std::string(name).find_last_of('_')-1); + std::string ns = std::string(name).substr(0, std::string(name).find_last_of('_')-1); // convert the name into a type std::string type_name; @@ -420,19 +446,30 @@ std::string generate(const char* definition, const char* name) for (auto& field: type.second->fields) { // dont allow strings yet - if (field.first->base_type == "char*") + if (field->type->base_type == "char*") { - printf("%s:%i ERROR: Strings not yet allowed in structs.\n", _current_file.c_str(), line_number); + printf("%s:%i ERROR: Dynamicly sized strings not allowed in structs.\n", _current_file.c_str(), line_number); throw 7; } - if (field.first->type_enum == "FT_Struct") + if (field->type->type_enum == "FT_Struct") { printf("%s:%i ERROR: Structs not yet allowed in structs.\n", _current_file.c_str(), line_number); throw 7; } - - output += " " + field.first->base_type + " " + field.second + ";\n"; + + if (field->type == types["astring"]) + { + output += " " + field->type->base_type + " " + field->name + "[" + std::to_string(field->string_size) + "];\n"; + } + else if (field->array_size > 1) + { + output += " " + field->type->base_type + " " + field->name + "[" + std::to_string(field->array_size) + "];\n"; + } + else + { + output += " " + field->type->base_type + " " + field->name + ";\n"; + } } output += "};\n\n"; } @@ -476,14 +513,14 @@ std::string generate(const char* definition, const char* name) // now add struct fields for (auto& m : members) { - output += " { " + m.first->type_enum + ", " + "FF_NONE" + ", \"" + m.second + "\", "; - output += std::to_string(1) + ", 0 }, \n";// todo support array members + output += " { " + m->type->type_enum + ", " + "FF_NONE" + ", \"" + m->name + "\", "; + output += std::to_string(m->array_size) + ", " + std::to_string(m->string_size) + " }, \n"; } } else { output += " { " + field.getTypeEnum() + ", " + field.getFlags() + ", \"" + field.name + "\", "; - output += std::to_string(field.array_size) + ", 0 }, \n";// todo use for array types + output += std::to_string(field.array_size) + ", " + std::to_string(field.string_size) + " }, \n"; } } output += "};\n\n"; @@ -794,16 +831,50 @@ std::string generate(const char* definition, const char* name) output += "#include \n"; output += "#include \n"; //output += "#include \n"; + // todo only include if needed output += "#include \n"; + output += "#include \n"; output += "namespace " + ns + "\n{\n"; output += "namespace msg\n{\n"; output += "#pragma pack(push, 1)\n"; output += "struct " + raw_name + "\n{\n"; + + // generate internal structs + for (auto& type: types) + { + if (type.second->type_enum != "FT_Struct") + { + continue; + } + + output += " struct " + type.second->name + "\n {\n"; + for (auto& field: type.second->fields) + { + if (field->type == types["astring"]) + { + output += " FixedString<" + std::to_string(field->string_size) + "> " + field->name + ";\n"; + } + else if (field->array_size > 1) + { + output += " " + field->type->base_type + " " + field->name + "[" + std::to_string(field->array_size) +"];\n"; + } + else + { + output += " " + field->type->base_type + " " + field->name + ";\n"; + } + } + type.second->base_type = type.second->name; + output += " };\n\n"; + } for (auto f: fields) { std::string type = f.type == string_type ? "char*" : f.getBaseType(); - if (f.array_size == 1) + if (f.type == string_type && f.array_size == 1) + { + output += " CString " + f.name + ";\n"; + } + else if (f.array_size == 1) { output += " " + type + " " + f.name + ";\n"; } diff --git a/tools/throughput_test.cpp b/tools/throughput_test.cpp index 0b8653f..ea10200 100644 --- a/tools/throughput_test.cpp +++ b/tools/throughput_test.cpp @@ -31,8 +31,7 @@ int main() // okay, since we are publishing with shared pointer we actually need to allocate the string properly auto shared = pubsub::msg::StringSharedPtr(new pubsub::msg::String); - shared->value = new char[strlen(msg.value) + 1]; - strcpy(shared->value, msg.value); + shared->value = msg.value; while (ps_okay()) { From de2dd7deb4bf97abd5b7719edceef692bbcdea7a Mon Sep 17 00:00:00 2001 From: Matthew B Date: Mon, 24 Feb 2025 21:49:33 -0800 Subject: [PATCH 24/34] Fix enumerations in structs Refactor structs in message definitions to reduce duplication --- include/pubsub/Serialization.h | 10 ++- src/Bindings.c | 2 +- src/Serialization.c | 144 ++++++++++++++++++--------------- tools/generator.cpp | 115 +++++++++++++++++++------- 4 files changed, 174 insertions(+), 97 deletions(-) diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index d0263ef..e3d6f0a 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -33,6 +33,7 @@ extern "C" FT_MaxFloat,// all floats and ints are less than this, not present in messages FT_String,// null terminated dynamic length string FT_Struct,//indicates the number of fields following contained in it + FT_StructDefinition, FT_ArrayString// null terminated fixed length string }; typedef enum ps_field_types ps_field_types; @@ -50,8 +51,13 @@ extern "C" ps_field_types type; ps_field_flags flags;// packed in upper bits of type, but broken out here const char* name; - unsigned int length;//length of the array, 0 if dynamic - unsigned short content_length;//number of fields inside this struct + uint32_t length;//length of the array, 0 if dynamic, or if this is a struct definition, how many following fields are part of it + //context dependent field + union + { + uint16_t string_length;// this field is a ArrayString, the length of said string + uint16_t struct_index;// if this field is a struct, this is the index of the struct definition in the list of fields + }; }; struct ps_msg_enum_t diff --git a/src/Bindings.c b/src/Bindings.c index 7c37145..ee9f574 100644 --- a/src/Bindings.c +++ b/src/Bindings.c @@ -161,7 +161,7 @@ EXPORT int ps_create_publisher(int node, const char* topic, const char* definiti struct ps_msg_field_t* field = &fields[num_fields-1]; field->name = name; field->length = 1; - field->content_length = 0; + field->string_length = 0; field->type = 0;// filled in below field->flags = 0; if (strcmp(type, "int8") == 0) diff --git a/src/Serialization.c b/src/Serialization.c index ff31332..4a09131 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -20,7 +20,7 @@ struct field { uint8_t type; uint32_t length; - uint16_t content_length;//if we are an array + uint16_t content_length;//context dependent char name[50]; //string goes here }; @@ -53,7 +53,7 @@ int ps_serialize_message_definition(void* start, const struct ps_message_definit struct field* f = (struct field*)cur; f->type = definition->fields[i].type | (definition->fields[i].flags << 5); f->length = definition->fields[i].length; - f->content_length = definition->fields[i].content_length; + f->content_length = definition->fields[i].struct_index; strcpy(f->name, definition->fields[i].name); cur += 1 + 4 + 2 + strlen(definition->fields[i].name)+ 1; @@ -92,7 +92,7 @@ void ps_copy_message_definition(struct ps_message_definition_t* dst, const struc dst->fields[i].type = src->fields[i].type; dst->fields[i].flags = src->fields[i].flags; dst->fields[i].length = src->fields[i].length; - dst->fields[i].content_length = src->fields[i].content_length; + dst->fields[i].struct_index = src->fields[i].struct_index; char* name = (char*)malloc(strlen(src->fields[i].name) + 1); strcpy(name, src->fields[i].name); dst->fields[i].name = name; @@ -137,7 +137,7 @@ void ps_deserialize_message_definition(const void * start, struct ps_message_def definition->fields[i].type = (ps_field_types)f->type & 0x1F; definition->fields[i].flags = f->type >> 5; definition->fields[i].length = f->length; - definition->fields[i].content_length = f->content_length; + definition->fields[i].struct_index = f->content_length; //need to allocate the name int len = strlen(f->name); char* field_name = (char*)malloc(len + 1); @@ -179,8 +179,9 @@ void ps_free_message_definition(struct ps_message_definition_t * definition) free(definition->enums); } -static int GetFieldSize(int type) +static int GetFieldSize(const struct ps_msg_field_t* field) { + int type = field->type; int field_size = 0; if (type == FT_Int8 || type == FT_UInt8) { @@ -198,6 +199,10 @@ static int GetFieldSize(int type) { field_size = 8; } + else if (type == FT_ArrayString) + { + field_size = field->string_length; + } return field_size; } @@ -252,6 +257,12 @@ const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const s } } } + else if (field->type == FT_StructDefinition) + { + // skip past it + iter->next_field_index += field->length; + return ps_deserialize_iterate(iter, f, l); + } else if (field->type == FT_Struct) { // lets just treat this as a normal element, and leave it to the iterator to handle this @@ -259,16 +270,17 @@ const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const s // okay, lets just not allow struct arrays in arrays for now? // TODO, i dont think this is even implemented in code gen + const struct ps_msg_field_t* struct_field = &iter->fields[field->struct_index]; + iter->struct_num_fields = struct_field->length; + // calculate element *width* uint32_t width = 0; - for (int i = 0; i < field->content_length; i++) + for (int i = 0; i < iter->struct_num_fields; i++) { - const struct ps_msg_field_t* member = &iter->fields[iter->next_field_index++]; - width += GetFieldSize(member->type); + const struct ps_msg_field_t* member = ++struct_field; + width += GetFieldSize(member)*member->length; } - iter->struct_num_fields = field->content_length; - if (field->length > 0) { // fixed array @@ -287,7 +299,7 @@ const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const s } else { - int field_size = GetFieldSize(field->type); + int field_size = GetFieldSize(field); // now handle length if (field->length > 0) @@ -308,7 +320,7 @@ const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const s return position; } -static uint64_t print_field(const struct ps_msg_field_t* field, const char** ptr) +static uint64_t print_field(const struct ps_msg_field_t* field, const char** ptr, const struct ps_message_definition_t* definition) { uint64_t value = 0; // non dynamic types @@ -364,11 +376,38 @@ static uint64_t print_field(const struct ps_msg_field_t* field, const char** ptr break; case FT_ArrayString: printf("\"%s\"", (char*)*ptr); - *ptr += field->content_length; + *ptr += field->string_length; break; default: printf("ERROR: unhandled field type when parsing....\n"); } + + if (field->flags == FF_ENUM) + { + const char* name = "Enum Not Found"; + for (unsigned int i = 0; i < definition->num_enums; i++) + { + if (&definition->fields[definition->enums[i].field] == field && value == definition->enums[i].value) + { + name = definition->enums[i].name; + } + } + printf(" (%s)", name); + } + else if (field->flags == FF_BITMASK) + { + const char* name = "Enum Not Found"; + printf(" ("); + for (unsigned int i = 0; i < definition->num_enums; i++) + { + if (&definition->fields[definition->enums[i].field] == field && (definition->enums[i].value & value) != 0) + { + name = definition->enums[i].name; + printf("%s, ", name); + } + } + printf(")"); + } return value; } @@ -432,14 +471,15 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ printf(" { "); for (int f = 0; f < iter.struct_num_fields; f++) { - const struct ps_msg_field_t* sf = (field+1+f); + const struct ps_msg_field_t* sf = definition->fields + 1 + field->struct_index + f; printf("%s: ", sf->name); if (sf->length > 1) { printf("["); for (int i = 0; i < sf->length; i++) { - print_field(sf, &ptr); + print_field(sf, &ptr, definition); + if (i != sf->length - 1) { printf(", "); @@ -449,7 +489,7 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ } else { - print_field(sf, &ptr); + print_field(sf, &ptr, definition); } if (f != (iter.struct_num_fields - 1)) { @@ -492,38 +532,7 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ for (unsigned int i = 0; i < length; i++) { - uint64_t value = 0; - value = print_field(field, &ptr); - - if (field->flags > 0) - { - if (field->flags == FF_ENUM) - { - const char* name = "Enum Not Found"; - for (unsigned int i = 0; i < definition->num_enums; i++) - { - if (definition->enums[i].field == it && value == definition->enums[i].value) - { - name = definition->enums[i].name; - } - } - printf(" (%s)", name); - } - else if (field->flags == FF_BITMASK) - { - const char* name = "Enum Not Found"; - printf(" ("); - for (unsigned int i = 0; i < definition->num_enums; i++) - { - if (definition->enums[i].field == it && (definition->enums[i].value & value) != 0) - { - name = definition->enums[i].name; - printf("%s, ", name); - } - } - printf(")"); - } - } + print_field(field, &ptr, definition); if (field->length == 1) { @@ -577,6 +586,8 @@ const char* TypeToString(ps_field_types type) return "struct"; case FT_ArrayString: return "astring"; + case FT_StructDefinition: + return "struct"; default: return "Unknown Type"; } @@ -591,42 +602,49 @@ void ps_print_definition(const struct ps_message_definition_t* definition, bool int end_tab = 0; for (unsigned int i = 0; i < definition->num_fields; i++) { + const struct ps_msg_field_t* field = &definition->fields[i]; + + const char* tab = i < end_tab ? " " : ""; + //print out any relevant enums for (unsigned int j = 0; j < definition->num_enums; j++) { if (definition->enums[j].field == i) { - printf("%s %i\n", definition->enums[j].name, definition->enums[j].value); + printf("%s%s %i\n", tab, definition->enums[j].name, definition->enums[j].value); } } - const char* type_name = ""; - ps_field_types type = definition->fields[i].type; - if (definition->fields[i].flags == FF_ENUM) + ps_field_types type = field->type; + const char* flag = ""; + if (field->flags == FF_ENUM) { - printf("enum "); + flag = "enum "; } - else if (definition->fields[i].flags == FF_BITMASK) + else if (field->flags == FF_BITMASK) { - printf("bitmask "); + flag = "bitmask "; } - const char* tab = i < end_tab ? " " : ""; - if (definition->fields[i].length > 1) + if (field->type == FT_StructDefinition) + { + printf("%s%s%s %s\n", tab, flag, TypeToString(field->type), field->name); + } + else if (field->length > 1) { - printf("%s%s %s[%i]\n", tab, TypeToString(definition->fields[i].type), definition->fields[i].name, definition->fields[i].length); + printf("%s%s%s %s[%i]\n", tab, flag, TypeToString(field->type), field->name, field->length); } - else if (definition->fields[i].length == 0) + else if (field->length == 0) { - printf("%s%s[] %s\n", tab, TypeToString(definition->fields[i].type), definition->fields[i].name);// dynamic array + printf("%s%s%s %s[]\n", tab, flag, TypeToString(field->type), field->name);// dynamic array } else { - printf("%s%s %s\n", tab, TypeToString(definition->fields[i].type), definition->fields[i].name); + printf("%s%s%s %s\n", tab, flag, TypeToString(field->type), field->name); } - if (definition->fields[i].type == FT_Struct) + if (field->type == FT_StructDefinition) { - end_tab = i + 1 + definition->fields[i].content_length; + end_tab = i + 1 + field->length; } } } diff --git a/tools/generator.cpp b/tools/generator.cpp index 86ddfe2..c1450d2 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -27,7 +27,6 @@ struct enumeration { std::string name; std::string value; - int field_num; }; struct field; @@ -55,13 +54,15 @@ struct field std::string flag; uint32_t line_number; + + std::vector associated_enums; - std::string getBaseType() + std::string getBaseType() const { return type->base_type; } - std::string getFlags() + std::string getFlags() const { if (flag == "enum") return "FF_ENUM"; @@ -75,12 +76,12 @@ struct field return "invalid"; } - std::string getTypeEnum() + std::string getTypeEnum() const { return type->type_enum; } - void GenerateFree(std::string& output) + void GenerateFree(std::string& output) const { if (type->name == "string" && array_size != 1) { @@ -104,7 +105,7 @@ struct field } } - void GenerateCopy(std::string& output, const std::string& source) + void GenerateCopy(std::string& output, const std::string& source) const { if (type->name == "string") { @@ -230,6 +231,7 @@ std::string generate(const char* definition, const char* name) types["astring"] = new Type{"astring", "char", "FT_ArrayString", {}}; // also generate the hash while we are at it + std::vector unassociated_enums; uint32_t hash = 0; uint32_t line_number = 0; for (auto& line : lines) @@ -299,6 +301,7 @@ std::string generate(const char* definition, const char* name) current_struct->name = name; current_struct->base_type = name; current_struct->type_enum = "FT_Struct"; + unassociated_enums.clear();// if they come before a struct defintion we arent sure what they go with continue; } @@ -332,11 +335,13 @@ std::string generate(const char* definition, const char* name) if (current_struct) { // this belongs in the struct - current_struct->fields.push_back(new field{ name, real_type, size, string_size, "", line_number }); + current_struct->fields.push_back(new field{ name, real_type, size, string_size, "", line_number, unassociated_enums }); + unassociated_enums.clear(); continue; } // also fill in array size - fields.push_back({ name, real_type, size, string_size, "", line_number }); + fields.push_back({ name, real_type, size, string_size, "", line_number, unassociated_enums }); + unassociated_enums.clear(); } // a line with flags maybe? else if (words.size() == 3 && !has_equal) @@ -374,12 +379,14 @@ std::string generate(const char* definition, const char* name) if (current_struct) { // this belongs in the struct - current_struct->fields.push_back(new field{ name, real_type, size, 0, "", line_number }); + current_struct->fields.push_back(new field{ name, real_type, size, 0, flag, line_number, unassociated_enums }); + unassociated_enums.clear(); continue; } // also fill in array size - fields.push_back({ name, real_type, size, 0, flag, line_number}); + fields.push_back({ name, real_type, size, 0, flag, line_number, unassociated_enums}); + unassociated_enums.clear(); } else { @@ -393,7 +400,8 @@ std::string generate(const char* definition, const char* name) std::string value = strip_whitespace(equals[1]); //printf("it was an enum: %s=%s\n", equals[0].c_str(), equals[1].c_str()); - enumerations.push_back({ name, value, fields.size() }); + enumerations.push_back({ name, value }); + unassociated_enums.push_back(enumerations.size() - 1); } else { @@ -501,27 +509,50 @@ std::string generate(const char* definition, const char* name) // generate the fields output += "static struct ps_msg_field_t " + type_name + "_fields[] = {\n"; - for (auto& field : fields) + std::map generated_structs; + std::map field_indexes; + int field_index = 0; + for (const auto& field : fields) { if (field.getTypeEnum() == "FT_Struct") { - // struct - auto& members = field.type->fields; - output += " { " + field.getTypeEnum() + ", " + field.getFlags() + ", \"" + field.name + "\", "; - output += std::to_string(field.array_size) + ", " + std::to_string(members.size()) + " }, \n";// todo use for array types + // add the struct if we haven't already + int struct_index; + if (generated_structs.find(field.type->name) == generated_structs.end()) + { + struct_index = field_index; + generated_structs[field.type->name] = struct_index; + + auto& members = field.type->fields; + // add the struct itself + output += " { FT_StructDefinition, FF_NONE, \"" + field.type->name + "\", "; + output += std::to_string(members.size()) + ", 0 }, \n"; + field_index++; + + // now add struct fields + for (auto& m : members) + { + output += " { " + m->type->type_enum + ", " + m->getFlags() + ", \"" + m->name + "\", "; + output += std::to_string(m->array_size) + ", " + std::to_string(m->string_size) + " }, \n"; + field_indexes[m] = field_index++; + } + } + else + { + struct_index = generated_structs[field.type->name]; + } - // now add struct fields - for (auto& m : members) - { - output += " { " + m->type->type_enum + ", " + "FF_NONE" + ", \"" + m->name + "\", "; - output += std::to_string(m->array_size) + ", " + std::to_string(m->string_size) + " }, \n"; - } + // add the field + output += " { " + field.getTypeEnum() + ", " + field.getFlags() + ", \"" + field.name + "\", "; + output += std::to_string(field.array_size) + ", " + std::to_string(struct_index) + " }, \n"; } else { + // add the field output += " { " + field.getTypeEnum() + ", " + field.getFlags() + ", \"" + field.name + "\", "; output += std::to_string(field.array_size) + ", " + std::to_string(field.string_size) + " }, \n"; } + field_indexes[&field] = field_index++; } output += "};\n\n"; @@ -529,9 +560,36 @@ std::string generate(const char* definition, const char* name) if (enumerations.size()) { output += "static struct ps_msg_enum_t " + type_name + "_enums[] = {\n"; - for (auto& e: enumerations) + int enum_id = 0; + for (const auto& e: enumerations) { - output += " {\"" + e.name + "\", " + e.value + ", " + std::to_string(e.field_num) + "},\n"; + // okay, lets flip the script, each field lists associated enums? + // now search for the associated field + int field_num = 255; + for (const auto& field: fields) + { + for (const auto& sf: field.type->fields) + { + for (auto id: sf->associated_enums) + { + if (id == enum_id) + { + field_num = field_indexes[sf]; + break; + } + } + } + for (auto id: field.associated_enums) + { + if (id == enum_id) + { + field_num = field_indexes[&field]; + break; + } + } + } + output += " {\"" + e.name + "\", " + e.value + ", " + std::to_string(field_num) + "},\n"; + enum_id++; } output += "};\n\n"; } @@ -811,19 +869,14 @@ std::string generate(const char* definition, const char* name) } // generate the actual message definition - int field_count = fields.size(); - for (auto& f: fields) - { - field_count += f.type->fields.size(); - } output += "static struct ps_message_definition_t " + type_name + "_def = { "; if (enumerations.size() == 0) { - output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, 0, 0 };\n"; + output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_index) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, 0, 0 };\n"; } else { - output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_count) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, " + std::to_string(enumerations.size()) + ", " + type_name + "_enums };\n"; + output += std::to_string(hash) + ", \"" + name + "\", " + std::to_string(field_index) + ", " + type_name + "_fields, " + type_name + "_encode, " + type_name + "_decode, " + type_name + "_free, " + std::to_string(enumerations.size()) + ", " + type_name + "_enums };\n"; } output += "\n#ifdef __cplusplus\n"; From 7ea7230ee1309a8a25f22b80ee59a707be2b8df1 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 25 Feb 2025 19:45:54 -0800 Subject: [PATCH 25/34] Cleanup a bit Add a test for a very complex struct Add test for array vectors --- include/pubsub/Serialization.h | 26 ++++---- include/pubsub_cpp/array_string.h | 15 +++-- src/Bindings.c | 2 +- src/Serialization.c | 6 +- tests/CMakeLists.txt | 2 +- tests/test_pubsub_c.cpp | 2 +- tests/test_serialization.cpp | 103 ++++++++++++++++++++++++++++++ tools/CMakeLists.txt | 9 +++ 8 files changed, 140 insertions(+), 25 deletions(-) diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index e3d6f0a..8f5c20e 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -8,14 +8,14 @@ extern "C" { #endif - struct ps_allocator_t - { - void*(*alloc)(unsigned int size, void* context); - void(*free)(void*, void* context); - void* context; - }; + struct ps_allocator_t + { + void*(*alloc)(unsigned int size, void* context); + void(*free)(void*, void* context); + void* context; + }; - extern struct ps_allocator_t ps_default_allocator; + extern struct ps_allocator_t ps_default_allocator; enum ps_field_types { @@ -103,15 +103,15 @@ extern "C" // Serializes a given message definition to a buffer. // Returns: Number of bytes written - int ps_serialize_message_definition(void* start, const struct ps_message_definition_t* definition); + int ps_serialize_message_definition(void* dst, const struct ps_message_definition_t* definition); // Deserializes a message definition from the specified buffer. - void ps_deserialize_message_definition(const void* start, struct ps_message_definition_t* definition); + void ps_deserialize_message_definition(const void* src, struct ps_message_definition_t* definition); // print out the deserialized contents of the message to console, for rostopic echo like implementations // in yaml format - // if field is non-null only print out the content of that field - void ps_deserialize_print(const void* data, const struct ps_message_definition_t* definition, unsigned int max_array_size, const char* field); + // if field_name is non-null only print out the content of that field + void ps_deserialize_print(const void* data, const struct ps_message_definition_t* definition, unsigned int max_array_size, const char* field_name); struct ps_deserialize_iterator { @@ -126,11 +126,11 @@ extern "C" // Create an iteratator to iterate through the fields of a serialized message // Returns: The iterator - struct ps_deserialize_iterator ps_deserialize_start(const char* msg, const struct ps_message_definition_t* definition); + struct ps_deserialize_iterator ps_deserialize_start(const void* msg, const struct ps_message_definition_t* definition); // Iterate through a serialized message one field at a time // Returns: Start pointer in the message for the current field or zero when at the end - const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const struct ps_msg_field_t** f, uint32_t* l); + const void* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const struct ps_msg_field_t** f, uint32_t* l); // Frees a dynamically allocated message definition void ps_free_message_definition(struct ps_message_definition_t* definition); diff --git a/include/pubsub_cpp/array_string.h b/include/pubsub_cpp/array_string.h index 853578b..6a84086 100644 --- a/include/pubsub_cpp/array_string.h +++ b/include/pubsub_cpp/array_string.h @@ -42,12 +42,17 @@ class FixedString strncpy(data_, string, string_length - 1); } - bool operator==(const char* other) + bool operator==(const char* other) const { return strcmp(other, data_) == 0; } - bool operator==(const std::string& other) + bool operator==(const std::string& other) const + { + return strcmp(other.c_str(), data_) == 0; + } + + bool operator==(const FixedString& other) const { return strcmp(other.c_str(), data_) == 0; } @@ -135,7 +140,7 @@ class CString } } - bool operator==(const char* other) + bool operator==(const char* other) const { if (data_ == 0) { @@ -144,7 +149,7 @@ class CString return strcmp(other, data_) == 0; } - bool operator==(const std::string& other) + bool operator==(const std::string& other) const { if (data_ == 0) { @@ -153,7 +158,7 @@ class CString return strcmp(other.c_str(), data_) == 0; } - bool operator==(const CString& other) + bool operator==(const CString& other) const { if (data_ == 0) { diff --git a/src/Bindings.c b/src/Bindings.c index ee9f574..bd41d27 100644 --- a/src/Bindings.c +++ b/src/Bindings.c @@ -226,7 +226,7 @@ EXPORT int ps_create_publisher(int node, const char* topic, const char* definiti EXPORT void ps_publish(int pub, const void* msg, int len) { - // publish the message simply since it is already encoded + // publish the message simply since it is already encoded struct ps_msg_t omsg; ps_msg_alloc(len, 0, &omsg); memcpy(ps_get_msg_start(omsg.data), msg, len); diff --git a/src/Serialization.c b/src/Serialization.c index 4a09131..937a759 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -206,7 +206,7 @@ static int GetFieldSize(const struct ps_msg_field_t* field) return field_size; } -struct ps_deserialize_iterator ps_deserialize_start(const char* msg, const struct ps_message_definition_t* definition) +struct ps_deserialize_iterator ps_deserialize_start(const void* msg, const struct ps_message_definition_t* definition) { struct ps_deserialize_iterator iter; iter.next_field_index = 0; @@ -217,7 +217,7 @@ struct ps_deserialize_iterator ps_deserialize_start(const char* msg, const struc } // takes in a deserialize iterator and returns pointer to the data and the current field -const char* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const struct ps_msg_field_t** f, uint32_t* l) +const void* ps_deserialize_iterate(struct ps_deserialize_iterator* iter, const struct ps_msg_field_t** f, uint32_t* l) { if (iter->next_field_index == iter->num_fields) { @@ -415,10 +415,8 @@ void ps_deserialize_print(const void * data, const struct ps_message_definition_ { struct ps_deserialize_iterator iter = ps_deserialize_start(data, definition); const struct ps_msg_field_t* field; uint32_t length; const char* ptr; - int it = -1; while (ptr = ps_deserialize_iterate(&iter, &field, &length)) { - it++; if (field_name && strcmp(field_name, field->name) != 0) { continue; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index eccfded..2e69dab 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -7,7 +7,7 @@ foreach(test_file ${tests}) add_executable("${test_file}" "${test_source}") target_include_directories("${test_file}" PUBLIC ../include) add_dependencies("${test_file}" pubsub pubsub_msgs) - target_link_libraries("${test_file}" pubsub ${CMAKE_THREAD_LIBS_INIT} pubsub_msgs) + target_link_libraries("${test_file}" pubsub ${CMAKE_THREAD_LIBS_INIT} pubsub_msgs pubsub_test_msgs) # Auto populate the tests from test source file # Note : CMake must be reconfigured if tests are added/renamed/removed from test source file diff --git a/tests/test_pubsub_c.cpp b/tests/test_pubsub_c.cpp index 849aaac..a3f87b1 100644 --- a/tests/test_pubsub_c.cpp +++ b/tests/test_pubsub_c.cpp @@ -174,7 +174,7 @@ TEST(test_publish_subscribe_large, []() { pubsub__PointCloud_free(data, &ps_default_allocator); free(message); }; - ps_node_create_subscriber_adv(&node, "/data", 0, &string_sub, &options); + ps_node_create_subscriber_adv(&node, "/data", &pubsub__PointCloud_def, &string_sub, &options); // now spin and wait for us to get the published message while (ps_okay() && !got_message) diff --git a/tests/test_serialization.cpp b/tests/test_serialization.cpp index 5220d11..86c8972 100644 --- a/tests/test_serialization.cpp +++ b/tests/test_serialization.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "mini_mock.hpp" @@ -41,6 +42,9 @@ TEST(test_string_cpp, []() { EXPECT(msg.value == value); EXPECT(msg.value == value.c_str()); EXPECT(strcmp(msg.value.c_str(), value.c_str()) == 0); + + // make sure the C++ string object is the same size as a pointer + EXPECT(sizeof(msg.value) == sizeof(char*)); } }); @@ -65,6 +69,25 @@ TEST(test_fixed_string_cpp, []() { } }); +TEST(test_array_vector_cpp, []() { + // test C++ array vectors + { + ArrayVector vector; + EXPECT(sizeof(vector) == 4 + sizeof(char*)); + EXPECT(vector.size() == 0); + vector.resize(4); + EXPECT(vector.size() == 4) + + for (auto& item: vector) + { + item = 55; + } + + EXPECT(vector[0] == 55); + EXPECT(vector[3] == 55); + } +}); + TEST(test_costmap_c_cpp, []() { // verify that the C type matches the C++ one memory wise pubsub::msg::Costmap msg; @@ -201,4 +224,84 @@ TEST(test_path2d_foreach, []() { delete out; }); +void compare_struct(const pubsub::msg::Test::Struct& a, const pubsub::msg::Test::Struct& b) +{ + EXPECT(a.test_array_string == b.test_array_string); + EXPECT(a.i1 == b.i1); + EXPECT(a.i2 == b.i2); + EXPECT(a.f3 == b.f3); + EXPECT(a.d4 == b.d4); + for (int i = 0; i < 3; i++) + EXPECT(a.a5[i] == b.a5[i]); +} + +void fill_struct(pubsub::msg::Test::Struct& a) +{ + a.test_array_string = "ok"; + a.i1 = rand(); + a.i2 = rand(); + a.f3 = rand()/100.0; + a.d4 = rand()/100.0; + for (int i = 0; i < 3; i++) + a.a5[i] = rand()/100.0; + a.test_enum = pubsub::msg::Test::TYPE_INT; +} + +TEST(test_complex_message, []() { + // try serializing then deserializing a message, making sure everything matches expectation + EXPECT(sizeof(pubsub::msg::Test) == sizeof(pubsub__Test)); + EXPECT(sizeof(pubsub::msg::Test::Struct) == 56); + pubsub::msg::Test msg; + msg.test_int = rand(); + msg.test_string = "THIS IS A STRING"; + fill_struct(msg.test_struct); + fill_struct(msg.test_structs[0]); + fill_struct(msg.test_structs[1]); + msg.test_struct_array.resize(4); + for (int i = 0; i < 4; i++) + fill_struct(msg.test_struct_array[i]); + msg.test_bitmask[0] = pubsub::msg::Test::TYPE2_FLAG1; + msg.test_bitmask[1] = pubsub::msg::Test::TYPE2_FLAG2; + msg.test_bitmask[2] = pubsub::msg::Test::TYPE2_FLAG1 | pubsub::msg::Test::TYPE2_FLAG2; + + ps_msg_t in = msg.Encode(); + EXPECT(in.len == 420);// 56*7 + 4 + 4 + 3 + 17 = 392 + 11 + 17 + + // Make sure deserialize iterators work correctly + struct ps_deserialize_iterator iter = ps_deserialize_start(ps_get_msg_start((const char*)in.data), pubsub::msg::Test::GetDefinition()); + const struct ps_msg_field_t* field; uint32_t length; const void* ptr; + std::vector fields; + while (ptr = ps_deserialize_iterate(&iter, &field, &length)) + { + fields.push_back(field->name); + + // check some values + if (strcmp(field->name, "test_bitmask") == 0) + { + EXPECT(*(uint8_t*)ptr == msg.test_bitmask[0]); + } + if (strcmp(field->name, "test_int") == 0) + { + EXPECT(*(uint32_t*)ptr == msg.test_int); + } + } + EXPECT(fields.size() == 6); + EXPECT(fields[0] == "test_int"); + EXPECT(fields[5] == "test_bitmask"); + + auto* out = pubsub::msg::Test::Decode(ps_get_msg_start(in.data)); + free(in.data); + + EXPECT(msg.test_int == out->test_int); + EXPECT(msg.test_string == out->test_string); + compare_struct(msg.test_struct, out->test_struct); + compare_struct(msg.test_structs[0], out->test_structs[0]); + compare_struct(msg.test_structs[1], out->test_structs[1]); + EXPECT(msg.test_struct_array.size() == out->test_struct_array.size()) + for (int i = 0; i < 4; i++) + compare_struct(msg.test_struct_array[i], out->test_struct_array[i]); + for (int i = 0; i < 3; i++) + EXPECT(msg.test_bitmask[i] == out->test_bitmask[i]); +}); + CREATE_MAIN_ENTRY_POINT(); diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 30c8e27..e49e123 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -54,6 +54,7 @@ macro(generate_messages) endmacro() generate_messages(FILES + Frames.msg Joy.msg String.msg Float.msg @@ -72,6 +73,14 @@ generate_messages(FILES Int.msg ) +generate_messages(FILES + Test.msg +TARGET + pubsub_test_msgs +DIRECTORY + tests +) + # Build the test program add_executable(pubsubtest "PubSubTest.cpp") From 5c9215dd32fcbf1d770e86a6bcefe33335fa768b Mon Sep 17 00:00:00 2001 From: Matthew B Date: Thu, 27 Feb 2025 23:59:34 -0800 Subject: [PATCH 26/34] Add allocator support to C++ message generation --- include/pubsub_cpp/allocator.h | 13 ++++++ include/pubsub_cpp/array_string.h | 49 +++++++++++++++++------ include/pubsub_cpp/array_vector.h | 52 +++++++++++++----------- tests/test_serialization.cpp | 66 +++++++++++++++++++++++++++++-- tools/generator.cpp | 25 +++++++----- 5 files changed, 156 insertions(+), 49 deletions(-) create mode 100644 include/pubsub_cpp/allocator.h diff --git a/include/pubsub_cpp/allocator.h b/include/pubsub_cpp/allocator.h new file mode 100644 index 0000000..bae8328 --- /dev/null +++ b/include/pubsub_cpp/allocator.h @@ -0,0 +1,13 @@ +#pragma once + +namespace pubsub +{ +struct DefaultAllocator +{ + static ps_allocator_t* allocator() + { + return &ps_default_allocator; + } +}; + +} diff --git a/include/pubsub_cpp/array_string.h b/include/pubsub_cpp/array_string.h index 6a84086..81dc79e 100644 --- a/include/pubsub_cpp/array_string.h +++ b/include/pubsub_cpp/array_string.h @@ -1,10 +1,14 @@ #pragma once +#include #include #include #include +namespace pubsub +{ + #pragma pack(push, 1) // Wrapper for a fixed size array for using it like a string template @@ -78,12 +82,33 @@ class FixedString } }; -//would have to require the allocator in this class as part of the template // Wrapper for a C string which makes it easier to use and handles freeing -// todo how to handle allocators? +template class CString { char* data_; + + // copy a buffer of a given length to a new allocated array + static char* copy(const char* obj, uint32_t length) + { + auto data = (char*)Allocator::allocator()->alloc(length, Allocator::allocator()->context); + for (int i = 0; i < length; i++) + { + data[i] = obj[i]; + } + return data; + } + + static char* copy(const char* obj) + { + auto length = strlen(obj) + 1; + auto data = (char*)Allocator::allocator()->alloc(length, Allocator::allocator()->context); + for (int i = 0; i < length; i++) + { + data[i] = obj[i]; + } + return data; + } public: CString() @@ -96,7 +121,7 @@ class CString data_ = 0; if (other.data_) { - data_ = strdup(other.data_); + data_ = copy(other.data_); } } @@ -104,7 +129,7 @@ class CString { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } } @@ -112,31 +137,30 @@ class CString { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } - data_ = (char*)malloc(string.length()+1); - strcpy(data_, string.c_str()); + data_ = copy(string.c_str(), string.length()+1); } void operator=(const char* string) { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } - data_ = (char*)malloc(strlen(string)+1); - strcpy(data_, string); + data_ = copy(string); } void operator=(const CString& other) { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); + data_ = 0; } if (other.data_) { - data_ = strdup(other.data_); + data_ = copy(other.data_); } } @@ -182,3 +206,4 @@ class CString } }; #pragma pack(pop) +} diff --git a/include/pubsub_cpp/array_vector.h b/include/pubsub_cpp/array_vector.h index 001d003..3961783 100644 --- a/include/pubsub_cpp/array_vector.h +++ b/include/pubsub_cpp/array_vector.h @@ -2,15 +2,30 @@ #pragma once #include +#include +namespace pubsub +{ #pragma pack(push, 1) // Vector like array that's able to take ownership of C malloced arrays -template +template class ArrayVector { T* data_; uint32_t length_; + + // copy a buffer of a given length to a new allocated array + static T* copy(const T* obj, uint32_t length) + { + auto data = (T*)Allocator::allocator()->alloc(sizeof(T)*length, Allocator::allocator()->context); + for (int i = 0; i < length; i++) + { + data[i] = obj[i]; + } + return data; + } + public: ArrayVector() @@ -28,34 +43,26 @@ class ArrayVector ArrayVector(const ArrayVector& obj) { length_ = obj.length_; - data_ = (T*)malloc(sizeof(T)*length_); - for (int i = 0; i < length_; i++) - { - data_[i] = obj[i]; - } + data_ = copy(obj.data(), length_); } ~ArrayVector() { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } } - ArrayVector& operator=(const ArrayVector& obj) + ArrayVector& operator=(const ArrayVector& arr) { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } - length_ = obj.length_; - data_ = (T*)malloc(sizeof(T)*length_); - for (int i = 0; i < length_; i++) - { - data_[i] = obj[i]; - } + length_ = arr.length_; + data_ = copy(arr.data(), length_); return *this; } @@ -63,15 +70,11 @@ class ArrayVector { if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } length_ = arr.size(); - data_ = (T*)malloc(sizeof(T)*length_); - for (uint32_t i = 0; i < length_; i++) - { - data_[i] = arr[i]; - } + data_ = copy(arr.data(), length_); return *this; } @@ -80,7 +83,7 @@ class ArrayVector { if (size == length_) { return; } - auto new_data = (T*)malloc(sizeof(T)*size); + auto new_data = (T*)Allocator::allocator()->alloc(sizeof(T)*size, Allocator::allocator()->context); auto copy_len = std::min(size, length_); for (uint32_t i = 0; i < copy_len; i++) { @@ -89,7 +92,7 @@ class ArrayVector length_ = size; if (data_) { - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); } data_ = new_data; } @@ -106,7 +109,7 @@ class ArrayVector void clear() { length_ = 0; - free(data_); + Allocator::allocator()->free(data_, Allocator::allocator()->context); data_ = 0; } @@ -127,3 +130,4 @@ class ArrayVector inline const_iterator end() const { return &data_[length_]; } }; #pragma pack(pop) +} diff --git a/tests/test_serialization.cpp b/tests/test_serialization.cpp index 86c8972..a99e61a 100644 --- a/tests/test_serialization.cpp +++ b/tests/test_serialization.cpp @@ -51,7 +51,7 @@ TEST(test_string_cpp, []() { TEST(test_fixed_string_cpp, []() { // test C++ fixed strings { - FixedString<5> string; + pubsub::FixedString<5> string; string = "hi"; EXPECT(string == "hi"); @@ -62,7 +62,7 @@ TEST(test_fixed_string_cpp, []() { // test what happens when you assign too much { - FixedString<5> string; + pubsub::FixedString<5> string; EXPECT_THROWS([&](){ string = "hello paul"; }, "Too big."); @@ -72,7 +72,7 @@ TEST(test_fixed_string_cpp, []() { TEST(test_array_vector_cpp, []() { // test C++ array vectors { - ArrayVector vector; + pubsub::ArrayVector vector; EXPECT(sizeof(vector) == 4 + sizeof(char*)); EXPECT(vector.size() == 0); vector.resize(4); @@ -304,4 +304,64 @@ TEST(test_complex_message, []() { EXPECT(msg.test_bitmask[i] == out->test_bitmask[i]); }); + +// tracking allocator usage +static int allocated; +static int freed; +static std::map sizes; +static ps_allocator_t alloc; +struct TestAllocator +{ + static ps_allocator_t* allocator() { + return &alloc; + } + + static void* Allocate(uint32_t size, void* context) + { + auto ptr = malloc(size); + sizes[ptr] = size; + allocated += size; + printf("allocate %i\n", allocated); + return ptr; + } + + static void Free(void* ptr, void* context) + { + freed += sizes[ptr]; + printf("free %i\n", freed); + free(ptr); + } + + static void Setup() + { + allocated = 0; + freed = 0; + alloc.context = 0; + alloc.free = Free; + alloc.alloc = Allocate; + } +}; + +TEST(test_message_allocators, []() { + // make sure the allocator gets used + TestAllocator::Setup(); + + auto msg = new pubsub::msg::Costmap_(); + msg->data.resize(1000); + + delete msg; + + EXPECT(allocated == 1041); + EXPECT(freed > 0); + EXPECT(freed == allocated); + + auto msg2 = new pubsub::msg::String_(); + msg2->value = "hi"; + + delete msg2; + + EXPECT(allocated == 1041+4+3+4); + EXPECT(freed == allocated); +}); + CREATE_MAIN_ENTRY_POINT(); diff --git a/tools/generator.cpp b/tools/generator.cpp index c1450d2..013614d 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -885,13 +885,15 @@ std::string generate(const char* definition, const char* name) output += "#include \n"; //output += "#include \n"; // todo only include if needed + output += "#include \n"; output += "#include \n"; output += "#include \n"; output += "namespace " + ns + "\n{\n"; output += "namespace msg\n{\n"; output += "#pragma pack(push, 1)\n"; - output += "struct " + raw_name + "\n{\n"; - + output += "template \n"; + output += "struct " + raw_name + "_\n{\n"; + output += " typedef std::shared_ptr<" + raw_name + "_> SharedPtr;\n"; // generate internal structs for (auto& type: types) { @@ -925,7 +927,7 @@ std::string generate(const char* definition, const char* name) std::string type = f.type == string_type ? "char*" : f.getBaseType(); if (f.type == string_type && f.array_size == 1) { - output += " CString " + f.name + ";\n"; + output += " CString " + f.name + ";\n"; } else if (f.array_size == 1) { @@ -933,7 +935,7 @@ std::string generate(const char* definition, const char* name) } else if (f.array_size == 0) { - output += " ArrayVector<" + type + "> " + f.name + ";\n"; + output += " ArrayVector<" + type + ", Allocator> " + f.name + ";\n"; } else { @@ -974,24 +976,27 @@ std::string generate(const char* definition, const char* name) output += " void* operator new(size_t size)\n"; output += " {\n"; //output += " std::cout<< \"Overloading new operator with size: \" << size << std::endl;\n"; - output += " return malloc(size);\n"; + output += " return Allocator::allocator()->alloc(size, Allocator::allocator()->context);\n"; + //output += " return malloc(size);\n"; output += " }\n\n"; output += " void operator delete(void * p)\n"; output += " {\n"; //output += " std::cout<< \"Overloading delete operator \" << std::endl;\n"; - output += " free(p);\n"; + output += " Allocator::allocator()->free(p, Allocator::allocator()->context);\n"; + //output += " free(p);\n"; output += " }\n\n"; output += " static const ps_message_definition_t* GetDefinition()\n {\n"; output += " return &" + type_name + "_def;\n }\n\n"; output += " ps_msg_t Encode() const\n {\n"; - output += " return " + ns + "__" + raw_name + "_encode(this, &ps_default_allocator);\n }\n\n"; - output += " static " + raw_name + "* Decode(const void* data)\n {\n"; - output += " return (" + raw_name + "*)" + ns + "__" + raw_name + "_decode(data, &ps_default_allocator);\n }\n";// + ns + "__" + raw_name + "_encode(0, this);\n }\n"; + output += " return " + ns + "__" + raw_name + "_encode(this, Allocator::allocator());\n }\n\n"; + output += " static " + raw_name + "_* Decode(const void* data)\n {\n"; + output += " return (" + raw_name + "_*)" + ns + "__" + raw_name + "_decode(data, Allocator::allocator());\n }\n";// + ns + "__" + raw_name + "_encode(0, this);\n }\n"; output += "};\n"; + output += "typedef " + raw_name + "_<> " + raw_name + ";\n"; + output += "typedef std::shared_ptr<" + raw_name + "_<>> " + raw_name + "SharedPtr;\n"; output += "#pragma pack(pop)\n"; - output += "typedef std::shared_ptr<" + raw_name + "> " + raw_name + "SharedPtr;\n"; output += "}\n"; output += "}\n"; From 07b52d92373eb0f773ecc39bd54b3a9e124bc471 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Fri, 28 Feb 2025 21:35:26 -0800 Subject: [PATCH 27/34] Integrate allocators into C++ --- include/pubsub/Node.h | 2 +- include/pubsub/Publisher.h | 18 +++++---- include/pubsub/Serialization.h | 4 +- include/pubsub/TCPTransport.h | 6 +-- include/pubsub_cpp/Node.h | 6 +-- src/Node.c | 5 ++- src/Publisher.c | 6 +-- src/Serialization.c | 12 +++--- tests/test_pubsub_cpp.cpp | 71 ++++++++++++++++++++++++++++++++++ tools/generator.cpp | 5 ++- tools/pubsub.cpp | 4 +- 11 files changed, 106 insertions(+), 33 deletions(-) diff --git a/include/pubsub/Node.h b/include/pubsub/Node.h index 8eae74c..8608a3b 100644 --- a/include/pubsub/Node.h +++ b/include/pubsub/Node.h @@ -196,7 +196,7 @@ void ps_node_init_ex(struct ps_node_t* node, const char* name, const char* ip, b void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched); -void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport); +void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport, struct ps_allocator_t* allocator); typedef void(*ps_subscriber_fn_cb_t)(void* message, unsigned int size, void* data, const struct ps_msg_info_t* info); diff --git a/include/pubsub/Publisher.h b/include/pubsub/Publisher.h index 8bc7807..6593c42 100644 --- a/include/pubsub/Publisher.h +++ b/include/pubsub/Publisher.h @@ -14,20 +14,20 @@ struct ps_message_definition_t; struct ps_endpoint_t { - unsigned short port; - int address; + unsigned short port; + int address; //bool multicast;// this is probably unnecessary }; // publisher client to network to struct ps_client_t { - struct ps_endpoint_t endpoint; - unsigned short sequence_number;// sequence of the networked packets, incremented with each one - unsigned long long last_keepalive;// timestamp of the last keepalive message, used to know when to deactiveate this connection - unsigned int stream_id;// user-unique identifier of what topic this came from - unsigned int modulo; - struct ps_transport_t* transport; + struct ps_endpoint_t endpoint; + unsigned short sequence_number;// sequence of the networked packets, incremented with each one + unsigned long long last_keepalive;// timestamp of the last keepalive message, used to know when to deactiveate this connection + unsigned int stream_id;// user-unique identifier of what topic this came from + unsigned int modulo; + struct ps_transport_t* transport; }; struct ps_pub_t @@ -39,6 +39,8 @@ struct ps_pub_t struct ps_node_t* node; unsigned int num_clients; struct ps_client_t* clients; + + struct ps_allocator_t* allocator; bool latched;// todo make this an enum of options if we add more uint8_t recommended_transport; diff --git a/include/pubsub/Serialization.h b/include/pubsub/Serialization.h index 8f5c20e..0ceb7cb 100644 --- a/include/pubsub/Serialization.h +++ b/include/pubsub/Serialization.h @@ -82,7 +82,7 @@ extern "C" }; void ps_msg_ref_add(struct ps_msg_ref_t* msg); - void ps_msg_ref_free(struct ps_msg_ref_t* msg); + void ps_msg_ref_free(struct ps_msg_ref_t* msg, struct ps_allocator_t* allocator); struct ps_allocator_t; typedef struct ps_msg_t(*ps_fn_encode_t)(const void* msg, struct ps_allocator_t* allocator); @@ -150,7 +150,7 @@ extern "C" // Makes a copy of a given serialized message // Returns: The new copy - struct ps_msg_t ps_msg_cpy(const struct ps_msg_t* msg); + struct ps_msg_t ps_msg_cpy(const struct ps_msg_t* msg, struct ps_allocator_t* allocator); #ifdef __cplusplus } diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index e7dca9c..144ad92 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -130,7 +130,7 @@ void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, s if (transport->clients[i].queued_message) { - ps_msg_ref_free(transport->clients[i].queued_message); + ps_msg_ref_free(transport->clients[i].queued_message, transport->clients[i].publisher->allocator); } // free queued messages @@ -138,7 +138,7 @@ void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, s { for (int j = 0; j < transport->clients[i].num_queued_messages; j++) { - ps_msg_ref_free(transport->clients[i].queued_messages[j].msg); + ps_msg_ref_free(transport->clients[i].queued_messages[j].msg, transport->clients[i].publisher->allocator); } free(transport->clients[i].queued_messages); } @@ -301,7 +301,7 @@ int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* no if (client->queued_message_written == client->queued_message_length) { //printf("Message sent.\n"); - ps_msg_ref_free(client->queued_message); + ps_msg_ref_free(client->queued_message, client->publisher->allocator); client->queued_message = 0; // we finished! check if there are more to send diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index fd28b83..9356184 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -4,7 +4,7 @@ #include #include #include - +#include #include #include @@ -310,7 +310,7 @@ class Publisher: public PublisherBase remapped_topic_ = handle_remap(real_topic, node.getNamespace()); node.lock_.lock(); - ps_node_create_publisher_ex(node.getNode(), remapped_topic_.c_str(), T::GetDefinition(), &publisher_, latched, preferred_transport); + ps_node_create_publisher_ex(node.getNode(), remapped_topic_.c_str(), T::GetDefinition(), &publisher_, latched, preferred_transport, T::Allocator::allocator()); node.lock_.unlock(); //add me to the publisher list @@ -576,7 +576,7 @@ class Subscriber: public SubscriberBase options.skip = skip; options.cb = cb2; options.cb_data = this; - options.allocator = 0; + options.allocator = T::Allocator::allocator(); options.ignore_local = true; options.preferred_transport = preferred_transport; diff --git a/src/Node.c b/src/Node.c index f404540..defbf90 100644 --- a/src/Node.c +++ b/src/Node.c @@ -142,7 +142,7 @@ void ps_node_advertise(struct ps_pub_t* pub) int sent_bytes = sendto(pub->node->socket, (const char*)data, off, 0, (struct sockaddr*)&address, sizeof(struct sockaddr_in)); } -void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport) +void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched, unsigned int recommended_transport, struct ps_allocator_t* allocator) { node->num_pubs++; struct ps_pub_t** old_pubs = node->pubs; @@ -164,13 +164,14 @@ void ps_node_create_publisher_ex(struct ps_node_t* node, const char* topic, cons pub->last_message = 0; pub->sequence_number = 0; pub->recommended_transport = recommended_transport; + pub->allocator = allocator ? allocator : &ps_default_allocator; ps_node_advertise(pub); } void ps_node_create_publisher(struct ps_node_t* node, const char* topic, const struct ps_message_definition_t* type, struct ps_pub_t* pub, bool latched) { - ps_node_create_publisher_ex(node, topic, type, pub, latched, 0); + ps_node_create_publisher_ex(node, topic, type, pub, latched, 0, 0); } // Setup Control-C handlers diff --git a/src/Publisher.c b/src/Publisher.c index d47460d..7fb671b 100644 --- a/src/Publisher.c +++ b/src/Publisher.c @@ -182,13 +182,13 @@ void ps_pub_publish(struct ps_pub_t* pub, struct ps_msg_t* msg) if (pub->last_message) { //free the old and add the new - ps_msg_ref_free(pub->last_message);// todo use allocator + ps_msg_ref_free(pub->last_message, pub->allocator); } pub->last_message = ref; } else { - ps_msg_ref_free(ref);// todo use allocator + ps_msg_ref_free(ref, pub->allocator); } } @@ -229,7 +229,7 @@ void ps_pub_destroy(struct ps_pub_t* pub) // free my latched message if (pub->last_message) { - ps_msg_ref_free(pub->last_message);// todo use allocator + ps_msg_ref_free(pub->last_message, pub->allocator); } pub->clients = 0; diff --git a/src/Serialization.c b/src/Serialization.c index 937a759..ba0ce56 100644 --- a/src/Serialization.c +++ b/src/Serialization.c @@ -33,8 +33,6 @@ struct enumeration }; #pragma pack(pop) - - int ps_serialize_message_definition(void* start, const struct ps_message_definition_t* definition) { //ok, write out number of fields @@ -665,21 +663,21 @@ void ps_msg_ref_add(struct ps_msg_ref_t* msg) msg->refcount++; } -void ps_msg_ref_free(struct ps_msg_ref_t* msg) +void ps_msg_ref_free(struct ps_msg_ref_t* msg, struct ps_allocator_t* allocator) { msg->refcount--; if (msg->refcount == 0) { - free(msg->data); - free(msg); + allocator->free(msg->data, allocator->context); + allocator->free(msg, allocator->context); } } -struct ps_msg_t ps_msg_cpy(const struct ps_msg_t* msg) +struct ps_msg_t ps_msg_cpy(const struct ps_msg_t* msg, struct ps_allocator_t* allocator) { struct ps_msg_t out; - ps_msg_alloc(msg->len, 0, &out); + ps_msg_alloc(msg->len, allocator, &out); memcpy(ps_get_msg_start(out.data), ps_get_msg_start(msg->data), msg->len); return out; } diff --git a/tests/test_pubsub_cpp.cpp b/tests/test_pubsub_cpp.cpp index c1ef742..334c7ba 100644 --- a/tests/test_pubsub_cpp.cpp +++ b/tests/test_pubsub_cpp.cpp @@ -173,4 +173,75 @@ TEST(test_publish_subscribe_cpp, []() { EXPECT(got_message); }); +// tracking allocator usage +static int allocated; +static int freed; +static std::map sizes; +static ps_allocator_t alloc; +struct TestAllocator +{ + static ps_allocator_t* allocator() { + return &alloc; + } + + static void* Allocate(uint32_t size, void* context) + { + auto ptr = malloc(size); + sizes[ptr] = size; + allocated += size; + printf("allocate %i\n", allocated); + return ptr; + } + + static void Free(void* ptr, void* context) + { + freed += sizes[ptr]; + printf("free %i\n", freed); + free(ptr); + } + + static void Setup() + { + allocated = 0; + freed = 0; + alloc.context = 0; + alloc.free = Free; + alloc.alloc = Allocate; + } +}; + +TEST(test_publish_subscribe_allocator_cpp, []() { + TestAllocator::Setup(); + // test that allocators are used with C++ + pubsub::Node node("simple_publisher"); + bool got_message = false; + { + pubsub::Publisher> string_pub(node, "/data"); + + pubsub::msg::String_ omsg; + omsg.value = "Hello"; + + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node); + + pubsub::Subscriber> subscriber(node, "/data", [&](const pubsub::msg::String_::SharedPtr& msg) { + printf("Got message %s in sub1\n", msg->value.c_str()); + EXPECT(omsg.value == msg->value); + spinner.stop(); + got_message = true; + }, 10); + + spinner.addTimer(0.1, [&]() + { + string_pub.publish(omsg); + }); + + spinner.run(); + } + EXPECT(got_message); + + EXPECT(allocated == 20); + EXPECT(allocated == freed); +}); + CREATE_MAIN_ENTRY_POINT(); diff --git a/tools/generator.cpp b/tools/generator.cpp index 013614d..90a7a47 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -891,9 +891,10 @@ std::string generate(const char* definition, const char* name) output += "namespace " + ns + "\n{\n"; output += "namespace msg\n{\n"; output += "#pragma pack(push, 1)\n"; - output += "template \n"; + output += "template \n"; output += "struct " + raw_name + "_\n{\n"; - output += " typedef std::shared_ptr<" + raw_name + "_> SharedPtr;\n"; + output += " typedef std::shared_ptr<" + raw_name + "_> SharedPtr;\n"; + output += " typedef AllocatorT Allocator;\n"; // generate internal structs for (auto& type: types) { diff --git a/tools/pubsub.cpp b/tools/pubsub.cpp index 4cfa7d4..04d6db1 100644 --- a/tools/pubsub.cpp +++ b/tools/pubsub.cpp @@ -492,7 +492,7 @@ int topic_pub(int num_args, char** args, ps_node_t* node) } // do initial publish - ps_msg_t cpy = ps_msg_cpy(&msg); + ps_msg_t cpy = ps_msg_cpy(&msg, 0); ps_pub_publish(&pub, &cpy); break; } @@ -516,7 +516,7 @@ int topic_pub(int num_args, char** args, ps_node_t* node) ps_node_spin(node); if (rate != 0 && remaining < pubsub::Duration(0.0)) { - ps_msg_t cpy = ps_msg_cpy(&msg); + ps_msg_t cpy = ps_msg_cpy(&msg, 0); ps_pub_publish(&pub, &cpy); next = next + pubsub::Duration(1.0/rate); } From 6a278a70c81d16f70c89f8184afc2eabd6ffe477 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 9 Mar 2025 21:48:33 -0700 Subject: [PATCH 28/34] Add missing test message --- tests/Test.msg | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 tests/Test.msg diff --git a/tests/Test.msg b/tests/Test.msg new file mode 100644 index 0000000..efb9c01 --- /dev/null +++ b/tests/Test.msg @@ -0,0 +1,25 @@ +# A message that tests a wide variety of things +int32 test_int + +string test_string + +struct Struct +astring:10 test_array_string +uint8 i1 +int64 i2 +float f3 +double d4 +double a5[3] + +TYPE_INT=0 +TYPE_DOUBLE=1 +enum uint8 test_enum +end_struct + +Struct test_struct +Struct test_structs[2] +Struct test_struct_array[] + +TYPE2_FLAG1=1 +TYPE2_FLAG2=2 +bitmask uint8 test_bitmask[3] From c3a822d105793da94e75158a4a2b22fdeee7714d Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 9 Mar 2025 21:54:09 -0700 Subject: [PATCH 29/34] Remove accidentally comitted file --- tools/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index e49e123..cf7315a 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -53,8 +53,7 @@ macro(generate_messages) target_include_directories(${ARG_TARGET} INTERFACE ${CMAKE_CURRENT_BINARY_DIR}/include/) endmacro() -generate_messages(FILES - Frames.msg +generate_messages(FILES Joy.msg String.msg Float.msg From ae6a745e7b52e8c037d7cd8a423d5a03e20b64f8 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 9 Mar 2025 21:56:02 -0700 Subject: [PATCH 30/34] Update example --- examples/simple_pub.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/simple_pub.c b/examples/simple_pub.c index fa269e6..cd49390 100644 --- a/examples/simple_pub.c +++ b/examples/simple_pub.c @@ -29,7 +29,7 @@ int main() ps_node_create_publisher_ex(&node, "/data"/*topic name*/, &pubsub__String_def/*message definition*/, &string_pub, - true/*true to "latch" the topic*/, 1); + true/*true to "latch" the topic*/, 1, NULL); // User is responsible for lifetime of the message they publish // Publish does a copy internally if necessary From 0e4c6edba9e7ec5d8059d0214123e80563f27547 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 13 Apr 2025 21:46:02 -0700 Subject: [PATCH 31/34] Add missing consts in time --- include/pubsub_cpp/Time.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/include/pubsub_cpp/Time.h b/include/pubsub_cpp/Time.h index bf07cf7..61783bf 100644 --- a/include/pubsub_cpp/Time.h +++ b/include/pubsub_cpp/Time.h @@ -38,17 +38,17 @@ class Duration } - bool operator<(const Duration& rhs) const// otherwise, both parameters may be const references + bool operator<(const Duration& rhs) const { return this->usec < rhs.usec; } - bool operator>(const Duration& rhs) const// otherwise, both parameters may be const references + bool operator>(const Duration& rhs) const { return this->usec > rhs.usec; } - double toSec() + double toSec() const { return usec / 1000000.0; } @@ -77,38 +77,38 @@ class Time } - Duration operator-(const Time& rhs) // otherwise, both parameters may be const references + Duration operator-(const Time& rhs) const { Duration out; out.usec = this->usec - rhs.usec; - return out; // return the result by value (uses move constructor) + return out; } - bool operator<(const Time& rhs) const// otherwise, both parameters may be const references + bool operator<(const Time& rhs) const { return this->usec < rhs.usec; } - bool operator<=(const Time& rhs) const// otherwise, both parameters may be const references + bool operator<=(const Time& rhs) const { return this->usec <= rhs.usec; } - bool operator>(const Time& rhs) const// otherwise, both parameters may be const references + bool operator>(const Time& rhs) const { return this->usec > rhs.usec; } - bool operator>=(const Time& rhs) const// otherwise, both parameters may be const references + bool operator>=(const Time& rhs) const { return this->usec >= rhs.usec; } - Time operator+(const Duration& rhs) // otherwise, both parameters may be const references + Time operator+(const Duration& rhs) const { Time out; out.usec = this->usec + rhs.usec; - return out; // return the result by value (uses move constructor) + return out; } static Time now() @@ -147,12 +147,12 @@ class Time #endif } - double toSec() + double toSec() const { return usec / 1000000.0; } - std::string toString() + std::string toString() const { time_t t = usec / 1000000;// toSec(); From 15b44665e5d90b1202947c7eec0c6697b465558a Mon Sep 17 00:00:00 2001 From: Matthew B Date: Sun, 13 Apr 2025 21:46:35 -0700 Subject: [PATCH 32/34] Add SharedConstPtr to generator and properly namespace some things --- tools/generator.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tools/generator.cpp b/tools/generator.cpp index 90a7a47..5e9b23c 100644 --- a/tools/generator.cpp +++ b/tools/generator.cpp @@ -894,6 +894,7 @@ std::string generate(const char* definition, const char* name) output += "template \n"; output += "struct " + raw_name + "_\n{\n"; output += " typedef std::shared_ptr<" + raw_name + "_> SharedPtr;\n"; + output += " typedef std::shared_ptr> SharedConstPtr;\n"; output += " typedef AllocatorT Allocator;\n"; // generate internal structs for (auto& type: types) @@ -908,7 +909,7 @@ std::string generate(const char* definition, const char* name) { if (field->type == types["astring"]) { - output += " FixedString<" + std::to_string(field->string_size) + "> " + field->name + ";\n"; + output += " pubsub::FixedString<" + std::to_string(field->string_size) + "> " + field->name + ";\n"; } else if (field->array_size > 1) { @@ -928,7 +929,7 @@ std::string generate(const char* definition, const char* name) std::string type = f.type == string_type ? "char*" : f.getBaseType(); if (f.type == string_type && f.array_size == 1) { - output += " CString " + f.name + ";\n"; + output += " pubsub::CString " + f.name + ";\n"; } else if (f.array_size == 1) { @@ -936,7 +937,7 @@ std::string generate(const char* definition, const char* name) } else if (f.array_size == 0) { - output += " ArrayVector<" + type + ", Allocator> " + f.name + ";\n"; + output += " pubsub::ArrayVector<" + type + ", Allocator> " + f.name + ";\n"; } else { @@ -997,6 +998,7 @@ std::string generate(const char* definition, const char* name) output += "};\n"; output += "typedef " + raw_name + "_<> " + raw_name + ";\n"; output += "typedef std::shared_ptr<" + raw_name + "_<>> " + raw_name + "SharedPtr;\n"; + output += "typedef std::shared_ptr> " + raw_name + "SharedConstPtr;\n"; output += "#pragma pack(pop)\n"; output += "}\n"; From be79986a160a2ad67e93f5b09cd064266fd13558 Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 3 Jun 2025 22:10:54 -0700 Subject: [PATCH 33/34] Add parameter example Add cpp file for TCP transport to prevent linker errors Use globals rather than statics to fix intraprocess transport across compilation units Add ability to remove from C++ subscriber queues rather than use callbacks --- CMakeLists.txt | 2 + examples/CMakeLists.txt | 5 + examples/params.cpp | 38 ++ examples/simple_pub.cpp | 194 ++++++-- high_level_api/Node.cpp | 9 + include/pubsub/Parameter.h | 11 +- include/pubsub/TCPTransport.h | 873 +-------------------------------- include/pubsub_cpp/Node.h | 166 ++++++- include/pubsub_cpp/Time.h | 39 ++ src/TCPTransport.c | 885 ++++++++++++++++++++++++++++++++++ tests/CMakeLists.txt | 4 +- 11 files changed, 1320 insertions(+), 906 deletions(-) create mode 100644 examples/params.cpp create mode 100644 src/TCPTransport.c diff --git a/CMakeLists.txt b/CMakeLists.txt index e9d89de..9bfa47a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,6 +26,7 @@ add_library (pubsub "src/System.c" "src/UDPTransport.c" "src/Parameter.c" + "src/TCPTransport.c" ) if (UNIX) target_link_libraries(pubsub pubsub_msgs) @@ -47,6 +48,7 @@ add_library (pubsub_shared SHARED "src/Serialization.c" "src/System.c" "src/UDPTransport.c" + "src/TCPTransport.c" "src/Bindings.c") if (UNIX) target_link_libraries(pubsub_shared PUBLIC pubsub_msgs) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 951415d..4e8a61e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -33,3 +33,8 @@ set_property(TARGET simple_sub_cpp PROPERTY CXX_STANDARD 11) add_executable(read_param "read_param.c") target_link_libraries(read_param pubsub pubsub_msgs) add_dependencies(read_param pubsub pubsub_msgs) + +add_executable(params_cpp "params.cpp") +target_link_libraries(params_cpp pubsub_cpp pubsub_msgs) +add_dependencies(params_cpp pubsub_cpp pubsub_msgs) +set_property(TARGET simple_sub_cpp PROPERTY CXX_STANDARD 11) diff --git a/examples/params.cpp b/examples/params.cpp new file mode 100644 index 0000000..4e96b55 --- /dev/null +++ b/examples/params.cpp @@ -0,0 +1,38 @@ +#include + +#include +#include + +#include + +int main() +{ + // Create the node + pubsub::Node node("simple_parameters"/*node name*/); + + // Adds TCP transport (optional) + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, node.getNode()); + ps_node_add_transport(node.getNode(), &tcp_transport); + + // Create the "spinner" which executes callbacks and timers in a background thread + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node);// Add the node to the spinner + + auto start = pubsub::Time::now();// Gets the current time + + auto parameter = node.parameter("test_1", 1.0, "Description."); + auto parameter2 = node.parameter("test_2", 3.0, "Description 2."); + + // Create a timer which will run at a prescribed interval + spinner.addTimer(1.0/*timer is run every this many seconds*/, [&]() + { + printf("Value: %f %f\n", (float)(double)parameter, (float)(double)parameter2); + }); + + // Wait for the spinner to exit (on control-c) + spinner.run(); + + return 0; +} + diff --git a/examples/simple_pub.cpp b/examples/simple_pub.cpp index 0e8a538..fe131ea 100644 --- a/examples/simple_pub.cpp +++ b/examples/simple_pub.cpp @@ -7,45 +7,185 @@ #include #include +#include +#include +#include +#include +#include +#include #include int main() { - // Create the node - pubsub::Node node("simple_publisher"/*node name*/); + pubsub::Node node("simple_publisher"); + + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, node.getNode()); + ps_node_add_transport(node.getNode(), &tcp_transport); - // Adds TCP transport (optional) - struct ps_transport_t tcp_transport; - ps_tcp_transport_init(&tcp_transport, node.getNode()); - ps_node_add_transport(node.getNode(), &tcp_transport); + pubsub::Publisher string_pub(node, "/data"); - // Create the publisher - pubsub::Publisher string_pub(node, "/data"/*topic name*/, - false/*true to "latch" the topic*/); + //string_pub.addCustomEndpoint(0x7FFFFFFF, 11312, 0); - // Create the "spinner" which executes callbacks and timers in a background thread - pubsub::BlockingSpinnerWithTimers spinner; - spinner.setNode(node);// Add the node to the spinner + pubsub::Publisher image_pub(node, "/image"); + + + pubsub::Publisher image_pub2(node, "/image2"); + + pubsub::Publisher costmap_pub(node, "/costmap"); + + pubsub::Publisher marker_pub(node, "/marker"); + + pubsub::Publisher pointcloud_pub(node, "/pointcloud"); - auto start = pubsub::Time::now();// Gets the current time + pubsub::Publisher pose_pub(node, "/pose"); - // Create a timer which will run at a prescribed interval - spinner.addTimer(1.0/*timer is run every this many seconds*/, [&]() - { - auto now = pubsub::Time::now(); + pubsub::Publisher param_pub(node, "/parameters", true); - // Build and publish the message - pubsub::msg::String msg; - char value[20]; - sprintf(value, "Hello %f", (now-start).toSec()); - msg.value = value; - string_pub.publish(msg); - }); + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node); - // Wait for the spinner to exit (on control-c) - spinner.run(); + // make a parameters message + pubsub::msg::Parameters p; + p.name_length = 3; + p.name = new char*[3]; + p.name[0] = "/test"; + p.name[1] = "/test1"; + p.name[2] = "/test3"; + p.type_length = 3; + p.type = new uint8_t[3]; + p.type[0] = 0; + p.type[1] = 1; + p.type[2] = 2; + p.value_length = 0; + p.min_length = 0; + p.max_length = 0; + param_pub.publish(p); + delete[] p.type; + delete[] p.name; + p.type_length = p.name_length = 0; + p.name = 0; + p.type = 0; - return 0; + int iteration = 0; + int i = 0; + spinner.addTimer(0.1, [&]() + { + pubsub::msg::String msg; + char value[20]; + sprintf(value, "Hello %i", i++); + msg.value = value; + string_pub.publish(msg); + + ps_sleep(rand()%8); + + //printf("%i clients\n", string_pub.getNumSubscribers()); + + // okay, since we are publishing with shared pointer we actually need to allocate the string properly + /*auto shared = pubsub::msg::StringSharedPtr(new pubsub::msg::String); + shared->value = new char[strlen(msg.value) + 1]; + strcpy(shared->value, msg.value); + string_pub.publish(shared);*/ + + msg.value = 0;// so it doesnt get freed by the destructor since we allocated it ourself + + // generate an image for testing + auto img = pubsub::msg::ImageSharedPtr(new pubsub::msg::Image); + img->width = 100; + img->type = pubsub::msg::Image::R8G8B8; + img->height = 100; + img->data_length = 100*100*3; + img->data = (uint8_t*)malloc(img->data_length); + for (int y = 0; y < 100; y++) + { + for (int x = 0; x < 100; x++) + { + img->data[y*100*3 + x*3] = x*2;// r + img->data[y*100*3 + x*3 + 1] = y*2;// g + img->data[y*100*3 + x*3 + 2] = 0;// b + } + } + image_pub.publish(img); + image_pub2.publish(img); + + auto map = pubsub::msg::CostmapSharedPtr(new pubsub::msg::Costmap); + map->frame = 0; + map->resolution = 0.5; + map->left = 0; + map->bottom = 0; + map->width = 100; + map->height = 100; + map->data_length = map->width*map->height; + map->data = (uint8_t*)malloc(map->data_length); + for (int i = 0; i < map->data_length; i++) + { + map->data[i] = rand()%255; + } + costmap_pub.publish(map); + + auto marker = pubsub::msg::MarkerSharedPtr(new pubsub::msg::Marker); + marker->frame = 1; + marker->id = 0; + marker->marker_type = 0; + marker->data_length = 5*4; + marker->data = (double*)malloc(sizeof(double)*marker->data_length); + for (int i = 0; i < marker->data_length; i += 4) + { + marker->data[i] = i*3; + marker->data[i+1] = i*3; + marker->data[i+2] = (i+4)*3; + marker->data[i+3] = (i+4)*3; + } + marker_pub.publish(marker); + + auto pcld = pubsub::msg::PointCloudSharedPtr(new pubsub::msg::PointCloud); + pcld->point_type = pubsub::msg::PointCloud::POINT_XYZI; + pcld->num_points = 10000; + pcld->data_length = pcld->num_points*4*4; + pcld->data = (uint8_t*)malloc(pcld->data_length); + for (int i = 0; i < pcld->num_points; i++) + { + float* pt = (float*)&pcld->data[i*4*4]; + pt[0] = rand()%100 - 50;//x + pt[1] = rand()%100 - 50;//y + pt[2] = rand()%100 - 50;//z + pt[3] = (rand()%30)/30.0;//i + } + pointcloud_pub.publish(pcld); + pointcloud_pub.publish(pcld); + pointcloud_pub.publish(pcld); + + marker = pubsub::msg::MarkerSharedPtr(new pubsub::msg::Marker); + marker->frame = 1; + marker->id = 1; + marker->marker_type = 1; + marker->data_length = 5*2 + 1; + marker->data = (double*)malloc(sizeof(double)*marker->data_length); + marker->data[0] = 5; + for (int i = 1; i < marker->data_length; i += 2) + { + marker->data[i] = -i*3; + marker->data[i+1] = -i*3; + } + marker_pub.publish(marker); + + auto pose = pubsub::msg::PoseSharedPtr(new pubsub::msg::Pose); + pose->latitude = 0.0; + pose->longitude = 0.0; + pose->z = 10.0; + pose->y = 100.0*sin(iteration*0.01); + pose->x = 100.0*cos(iteration*0.01); + pose->odom_yaw = iteration*0.01; + pose->odom_pitch = 0; + pose->odom_roll = 0; + pose_pub.publish(pose); + + iteration++; + }); + + spinner.wait(); + + return 0; } diff --git a/high_level_api/Node.cpp b/high_level_api/Node.cpp index 7c83d0f..c3aef48 100644 --- a/high_level_api/Node.cpp +++ b/high_level_api/Node.cpp @@ -1 +1,10 @@ #include + +namespace pubsub +{ + std::mutex _publisher_mutex; + std::multimap _publishers; + std::multimap _subscribers; + + std::map _remappings; +} diff --git a/include/pubsub/Parameter.h b/include/pubsub/Parameter.h index 055abcf..ee1a561 100644 --- a/include/pubsub/Parameter.h +++ b/include/pubsub/Parameter.h @@ -1,11 +1,5 @@ #pragma once -#ifdef __cplusplus -extern "C" -{ -#endif - -//#include #include #include @@ -17,6 +11,11 @@ extern "C" #include +#ifdef __cplusplus +extern "C" +{ +#endif + typedef void(*ps_param_fancy_cb_t)(const char* name, double value, void* data); struct ps_parameters diff --git a/include/pubsub/TCPTransport.h b/include/pubsub/TCPTransport.h index 144ad92..596f6ac 100644 --- a/include/pubsub/TCPTransport.h +++ b/include/pubsub/TCPTransport.h @@ -1,6 +1,13 @@ #ifndef _PUBSUB_TCP_TRANSPORT_HEADER #define _PUBSUB_TCP_TRANSPORT_HEADER +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + // Must be included first on windows #include @@ -105,872 +112,12 @@ struct ps_tcp_transport_impl int num_connections; }; -void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, struct ps_node_t* node) -{ - // find the index - int i = 0; - for (; i < transport->num_clients; i++) - { - if (transport->clients[i].socket == socket)// socket packed in address - { - break; - } - } - -#ifdef _WIN32 - closesocket(socket); -#else - close(socket); -#endif - - if (transport->clients[i].packet_data) - { - free(transport->clients[i].packet_data); - } - - if (transport->clients[i].queued_message) - { - ps_msg_ref_free(transport->clients[i].queued_message, transport->clients[i].publisher->allocator); - } - - // free queued messages - if (transport->clients[i].num_queued_messages) - { - for (int j = 0; j < transport->clients[i].num_queued_messages; j++) - { - ps_msg_ref_free(transport->clients[i].queued_messages[j].msg, transport->clients[i].publisher->allocator); - } - free(transport->clients[i].queued_messages); - } - - struct ps_tcp_client_t* old_clients = transport->clients; - transport->num_clients -= 1; - - // close the socket and dont wait on it anymore - ps_event_set_remove_socket(&node->events, transport->clients[i].socket); - - if (transport->num_clients) - { - transport->clients = (struct ps_tcp_client_t*)malloc(sizeof(struct ps_tcp_client_t) * transport->num_clients); - for (int j = 0; j < i; j++) - { - transport->clients[j] = old_clients[j]; - } - - for (int j = i + 1; j <= transport->num_clients; j++) - { - transport->clients[j - 1] = old_clients[j]; - } - } - free(old_clients); -} - -void ps_tcp_remove_connection(struct ps_tcp_transport_impl* impl, int index) -{ - // Free our subscribers and any buffers - int iter = 0; - int new_size = impl->num_connections - 1; - struct ps_tcp_transport_connection* new_connections = new_size == 0 ? 0 : (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * new_size); - for (int i = 0; i < impl->num_connections; i++) - { - if (i != index) - { - new_connections[iter++] = impl->connections[i]; - continue; - } - - if (!impl->connections[i].waiting_for_header) - { - free(impl->connections[i].packet_data); - } - ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); -#ifdef _WIN32 - closesocket(impl->connections[i].socket); -#else - close(impl->connections[i].socket); -#endif - } - impl->num_connections = new_size; - free(impl->connections); - impl->connections = new_connections; -} - -int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* node) -{ - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; - int socket = accept(impl->socket, 0, 0); - if (socket > 0) - { -#ifdef PUBSUB_VERBOSE - printf("Got new socket connection!\n"); -#endif - - // add it to the list yo - impl->num_clients++; - struct ps_tcp_client_t* old_sockets = impl->clients; - impl->clients = (struct ps_tcp_client_t*)malloc(sizeof(struct ps_tcp_client_t) * impl->num_clients); - for (int i = 0; i < impl->num_clients - 1; i++) - { - impl->clients[i] = old_sockets[i]; - } - struct ps_tcp_client_t* new_client = &impl->clients[impl->num_clients - 1]; - new_client->socket = socket; - new_client->needs_removal = false; - new_client->current_packet_size = 0; - new_client->desired_packet_size = 0; - new_client->packet_data = 0; - new_client->queued_message = 0; - new_client->queued_message_length = 0; - new_client->queued_message_written = 0; - new_client->queued_messages = 0; - new_client->num_queued_messages = 0; - - // set non-blocking -#ifdef _WIN32 - DWORD nonBlocking = 1; - if (ioctlsocket(socket, FIONBIO, &nonBlocking) != 0) - { - printf("Failed to Set Socket as Non-Blocking!\n"); - closesocket(socket); - return 0; - } -#endif -#ifdef ARDUINO - fcntl(socket, F_SETFL, O_NONBLOCK); -#endif -#ifdef __unix__ - int flags = fcntl(socket, F_GETFL); - fcntl(socket, F_SETFL, flags | O_NONBLOCK); -#endif - - ps_event_set_add_socket(&node->events, socket); - - if (impl->num_clients - 1) - { - free(old_sockets); - } - } - //printf("polled\n"); - -// remove any old sockets - for (int i = 0; i < impl->num_clients; i++) - { - if (impl->clients[i].needs_removal) - { - // add it to the list - struct ps_client_t client; - client.endpoint.address = impl->clients[i].socket; - client.endpoint.port = 255;// p->port; - client.stream_id = 0; - ps_pub_remove_client(impl->clients[i].publisher, &client);// todo this is probably unsafe.... - - remove_client_socket(impl, impl->clients[i].socket, impl->clients[i].publisher->node); - - i = i - 1; - break; - } - } - - // update our sockets yo - for (int i = 0; i < impl->num_clients; i++) - { - struct ps_tcp_client_t* client = &impl->clients[i]; - - // send queued messages until we block or cant send anymore - while (client->queued_message != 0) - { - int to_send = client->queued_message_length - client->queued_message_written; - char* data = (char*)client->queued_message->data; - int sent = send(client->socket, &data[client->queued_message_written], to_send, 0); - if (sent > 0) - { - client->queued_message_written += sent; - } -#ifdef WIN32 - else if (sent < 0 && WSAGetLastError() != WSAEWOULDBLOCK) -#else - else if (sent < 0 && errno != EAGAIN) -#endif - { - //printf("needs removal %i\n", errno); - client->needs_removal = true; - } - - //printf("Sending more: %i to make %i of %i\n", sent, client->queued_message_written, client->queued_message_length); - - if (client->queued_message_written == client->queued_message_length) - { - //printf("Message sent.\n"); - ps_msg_ref_free(client->queued_message, client->publisher->allocator); - client->queued_message = 0; - - // we finished! check if there are more to send - if (client->num_queued_messages > 0) - { - // grab a message from the front of our message queue - client->queued_message = client->queued_messages[0].msg; - client->queued_message_written = 0; - client->queued_message_length = client->queued_messages[0].msg->len + sizeof(struct ps_msg_header); - - client->num_queued_messages -= 1; - if (client->num_queued_messages == 0) - { - free(client->queued_messages); - client->queued_messages = 0; - continue; - } - - struct ps_tcp_client_queued_message_t* msgs = (struct ps_tcp_client_queued_message_t*)malloc(client->num_queued_messages * sizeof(struct ps_tcp_client_queued_message_t)); - for (int i = 0; i < client->num_queued_messages; i++) - { - msgs[i] = client->queued_messages[i + 1];// take from the front - } - free(client->queued_messages); - client->queued_messages = msgs; - - // continue so we can attempt to send again - } - else - { - ps_event_set_remove_socket_write(&node->events, client->socket); - break;// no more to send - } - } - else - { - break;// we couldnt send anymore atm - } - } - - // check for new data and add it to the packet if present - char buf[1500]; - // if we havent gotten a header yet, just check for that - if (client->desired_packet_size == 0) - { - const int header_size = sizeof(struct ps_msg_header); - int len = recv(client->socket, buf, header_size, MSG_PEEK); - //printf("peek %i desired size %i\n", len, header_size); - if (len == 0) - { - client->needs_removal = true; - continue; - } - if (len < header_size) - { - continue;// no header yet - } - - char message_type = buf[0];// not used atm - - // we actually got the header! start looking for the message - len = recv(client->socket, buf, header_size, 0); - //connection->packet_type = message_type; - //printf("recv %i from client->socket desired size 0 2\n", len); - //client->waiting_for_header = false; - client->desired_packet_size = *(uint32_t*)&buf[1]; - //printf("Incoming message with %i bytes\n", client->desired_packet_size); - client->packet_data = (char*)malloc(client->desired_packet_size); - - client->current_packet_size = 0; - } - // read in the message - if (client->desired_packet_size != 0) - { - int remaining_size = client->desired_packet_size - client->current_packet_size; - // check for new messages and read until we hit packet size - int len = recv(client->socket, &client->packet_data[client->current_packet_size], remaining_size, 0); - //printf("recv %i from client->socket\n", len); - if (len > 0) - { - //printf("Read %i bytes of message\n", len); - client->current_packet_size += len; - - if (client->current_packet_size == client->desired_packet_size) - { -#ifdef PUBSUB_VERBOSE - printf("message finished\n"); -#endif - - if (true)// todo look at message id - { - // its a subscribe - const char* topic = &client->packet_data[4]; - // check if this matches any of our publishers - for (unsigned int pi = 0; pi < node->num_pubs; pi++) - { - struct ps_pub_t* pub = node->pubs[pi]; - if (strcmp(topic, pub->topic) == 0) - { - uint32_t skip = *(uint32_t*)&client->packet_data[0]; - // send response and start publishing - struct ps_client_t sub_client; - sub_client.endpoint.address = client->socket; - sub_client.endpoint.port = 255;// p->port; - sub_client.last_keepalive = 10000000000000;//GetTickCount64();// use the current time stamp - sub_client.sequence_number = 0; - sub_client.stream_id = 0; - sub_client.modulo = skip > 0 ? skip + 1 : 0; - sub_client.transport = transport; - - impl->clients[i].publisher = pub; - - // send the client the acknowledgement and message definition - char buf[1500]; - int32_t length = ps_serialize_message_definition((void*)buf, pub->message_definition); - struct ps_msg_header hdr; - hdr.pid = PS_TCP_PROTOCOL_MESSAGE_DEFINITION;// message definition - hdr.length = length; - hdr.id = hdr.seq = 0; - send(impl->clients[i].socket, (char*)&hdr, sizeof(hdr), 0); - send(impl->clients[i].socket, buf, length, 0); - -#ifdef PUBSUB_VERBOSE - printf("TCPTransport: Got subscribe request, adding client if we haven't already\n"); -#endif - ps_pub_add_client(pub, &sub_client); - - break; - } - } - } - - free(client->packet_data); - client->packet_data = 0; - client->desired_packet_size = 0; - } - } - } - } - - int message_count = 0; - for (int i = 0; i < impl->num_connections; i++) - { - struct ps_tcp_transport_connection* connection = &impl->connections[i]; - char buf[1500]; - if (connection->connecting) - { - //printf("checking for connected\n"); - // select to check for writability - fd_set wfds; - struct timeval tv; - int retval; - - FD_ZERO(&wfds); - FD_SET(connection->socket, &wfds); - - tv.tv_sec = 0; - tv.tv_usec = 0; - retval = select(connection->socket + 1, NULL, &wfds, NULL, &tv); - //printf("select\n"); - if (retval == -1) - { - // error? - printf("socket errored while connecting\n"); - } - else if (retval) - { - // socket is writable - //printf("socket writable\n"); - - // make the subscribe request in a "packet" - // a packet is an int length followed by data - int32_t length = strlen(connection->subscriber->topic) + 1 + 4; - - struct ps_msg_header hdr; - hdr.pid = 0x01; - hdr.length = length; - hdr.id = hdr.seq = 0; - send(connection->socket, (char*)&hdr, sizeof(hdr), 0); - - // make the request - uint32_t skip = connection->subscriber->skip; - send(connection->socket, (char*)&skip, 4, 0); - send(connection->socket, connection->subscriber->topic, length - 4, 0); - - connection->connecting = false; - } - } - // if we havent gotten a header yet, just check for that - else if (connection->waiting_for_header) - { - const int header_size = sizeof(struct ps_msg_header); - int len = recv(connection->socket, buf, header_size, MSG_PEEK); - //printf("peek got: %i\n", len); - if (len == 0) - { - // we got disconnected - ps_tcp_remove_connection(impl, i); - i--; - continue; - } - else if (len < header_size) - { - continue;// no header yet - } - - char message_type = buf[0]; - - // we actually got the header! start looking for the message - len = recv(connection->socket, buf, header_size, 0); - connection->packet_type = message_type; - connection->waiting_for_header = false; - connection->packet_size = *(uint32_t*)&buf[1]; - //printf("Incoming message with %i bytes\n", impl->connections[i].packet_size); - connection->packet_data = (char*)connection->subscriber->allocator->alloc(connection->packet_size, connection->subscriber->allocator->context); - - connection->current_size = 0; - } - else // read in the message - { - int remaining_size = connection->packet_size - connection->current_size; - - // check for new messages and read until we hit packet size - int len = recv(connection->socket, &connection->packet_data[connection->current_size], remaining_size, 0); - //printf("len %i\n", len); - if (len == 0) - { - // we got disconnected - ps_tcp_remove_connection(impl, i); - i--; - continue; - } - else if (len > 0) - { - connection->current_size += len; - //printf("Read %i bytes of message, so far: %i\n", len, connection->current_size); - - if (connection->current_size == connection->packet_size) - { - //printf("message finished type %x\n", connection->packet_type); - if (connection->packet_type == PS_TCP_PROTOCOL_MESSAGE_DEFINITION) - { - //printf("Was message definition\n"); - if (connection->subscriber->type == 0) - { - // todo put this in a function so we cant accidentally forget it - if (connection->subscriber->received_message_def.fields == 0) - { - ps_deserialize_message_definition(connection->packet_data, &connection->subscriber->received_message_def); - } - - // call the callback as well - if (node->def_cb) - { - node->def_cb(&connection->subscriber->received_message_def, node->def_cb_data); - } - } - - connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); - } - else if (connection->packet_type == PS_TCP_PROTOCOL_DATA) - { - //printf("added to queue\n"); - // decode and add it to the queue - struct ps_msg_info_t message_info; - message_info.address = connection->endpoint.address; - message_info.port = connection->endpoint.port; - - ps_sub_receive(connection->subscriber, connection->packet_data, connection->packet_size, false, &message_info); - - // remove the reference to packet data so we don't try and double free it on destroy - connection->packet_data = 0; - message_count++; - } - else - { - // unhandled packet id - connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); - } - connection->waiting_for_header = true; - } - } - } - } - return message_count; -} - -void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, struct ps_msg_ref_t* msg) -{ - // todo dont - int length = msg->len; - void* message = msg->data; - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; - - // the client packs the socket id in the addr - int socket = client->endpoint.address; - - // okay, so new version, if any write fails (EAGAIN or < expected size) - // we make a copy of the entire message and store it on that client to try and send again in our update loop - // if we get into this function and this client already has a queued message, just drop this one - struct ps_tcp_client_t* tclient = 0; - for (int i = 0; i < impl->num_clients; i++) - { - if (impl->clients[i].socket == socket) - { - tclient = &impl->clients[i]; - break; - } - } +void ps_tcp_transport_destroy(struct ps_transport_t* transport); - if (tclient->queued_message != 0) - { - // check if we have queue space left +void ps_tcp_transport_init(struct ps_transport_t* transport, struct ps_node_t* node); - // for now hardcode max queue size - const int max_queue_size = 10; - - // add a reference to the message and queue it up - ps_msg_ref_add(msg); - - // this if statement is unnecessary, but I added it for the sake of testing/completeness - if (tclient->queued_message == 0) - { - tclient->queued_message = msg; - tclient->queued_message_length = length + sizeof(struct ps_msg_header); - tclient->queued_message_written = 0; - } - else if (tclient->num_queued_messages >= max_queue_size) - { - // todo use a deque lol - // swap everything down, freeing the first - for (int i = tclient->num_queued_messages - 1; i >= 1; i--) - { - tclient->queued_messages[i] = tclient->queued_messages[i - 1]; - } - tclient->queued_messages[0].msg = msg; - printf("dropped message on topic '%s'\n", publisher->topic); - return;// drop it, we are out of queue space - } - else - { - //printf("queuing up message %i on topic '%s'\n", tclient->num_queued_messages, publisher->topic); - - // add the message to the front of the queue - tclient->num_queued_messages += 1; - struct ps_tcp_client_queued_message_t* msgs = (struct ps_tcp_client_queued_message_t*)malloc(tclient->num_queued_messages * sizeof(struct ps_tcp_client_queued_message_t)); - - msgs[0].msg = msg; - for (int i = 0; i < tclient->num_queued_messages - 1; i++) - { - msgs[i + 1] = tclient->queued_messages[i]; - } - free(tclient->queued_messages); - tclient->queued_messages = msgs; - - return; - } - } - //printf("started writing\n"); - // try and write, if any of these fail, make a copy - - // the message header is already filled out with the packet id and length - - int32_t desired_len = sizeof(struct ps_msg_header) + length; - //printf("trying to send message of %i bytes\n", desired_len); - int32_t c = send(socket, (char*)message, desired_len, 0); - if (c < desired_len && c >= 0) - { - tclient->queued_message_written = c; - goto FAILCOPY; - } - if (c < 0) - { -#ifdef WIN32 - int error = WSAGetLastError(); - if (error == WSAEWOULDBLOCK) -#else - if (errno == EAGAIN || errno == EWOULDBLOCK) -#endif - { - tclient->queued_message_written = 0; - goto FAILCOPY; - } - goto FAILDISCONNECT; - } - - //printf("wrote all\n"); - return; - - char* data; -FAILDISCONNECT: - //printf("Disconnected: %s\n", strerror(err)); - tclient->needs_removal = true; - return; - -FAILCOPY: - // add a reference count and put it in our queue - ps_msg_ref_add(msg); - - //printf("Wrote %i bytes\n", tclient->queued_message_written); - - tclient->queued_message = msg; - tclient->queued_message_length = length + sizeof(struct ps_msg_header); - ps_event_set_add_socket_write(&publisher->node->events, socket); - return; -} - -void ps_tcp_transport_subscribe(struct ps_transport_t* transport, struct ps_sub_t* subscriber, struct ps_endpoint_t* ep, uint32_t transport_info) -{ - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; - - // check if we already have a sub for this subscriber with this endpoint - // if so, ignore it - for (int i = 0; i < impl->num_connections; i++) - { - if (impl->connections[i].endpoint.port == ep->port && - impl->connections[i].endpoint.address == ep->address && - impl->connections[i].subscriber->sub_id == subscriber->sub_id) - { - return; - } - } - -#ifdef _WIN32 - SOCKET sock = socket(AF_INET, SOCK_STREAM, 0); -#else - int sock = socket(AF_INET, SOCK_STREAM, 0); -#endif - - // set non-blocking -#ifdef _WIN32 - DWORD nonBlocking = 1; - if (ioctlsocket(sock, FIONBIO, &nonBlocking) != 0) - { - ps_print_socket_error("Failed to Set Socket as Non-Blocking"); - closesocket(sock); - return; - } -#endif -#ifdef ARDUINO - fcntl(sock, F_SETFL, O_NONBLOCK); -#endif -#ifdef __unix__ - int flags = fcntl(sock, F_GETFL); - fcntl(sock, F_SETFL, flags | O_NONBLOCK); -#endif - - // Actually connect - //printf("connecting\n"); - struct sockaddr_in server_addr; - server_addr.sin_family = AF_INET; - server_addr.sin_addr.s_addr = htonl(ep->address); - server_addr.sin_port = htons(transport_info); - int connect_result = connect(sock, (struct sockaddr*)&server_addr, sizeof(server_addr)); - if (connect_result != 0) - { -#ifdef _WIN32 - if (WSAGetLastError() != WSAEWOULDBLOCK)//WSAEINPROGRESS -#else - if (errno != EINPROGRESS) -#endif - { - ps_print_socket_error("error connecting tcp socket"); - return; - } - } - - //printf("%i %i %i %i\n", (ep->address & 0xFF000000) >> 24, (ep->address & 0xFF0000) >> 16, (ep->address & 0xFF00) >> 8, (ep->address & 0xFF)); - - // make the subscribe request in a "packet" - // a packet is an int length followed by data - /*int8_t packet_type = 0x01;//subscribe - send(sock, (char*)&packet_type, 1, 0); - - int32_t length = strlen(subscriber->topic) + 1 + 4; - send(sock, (char*)&length, 4, 0); - - // make the request - char buffer[500]; - strcpy(buffer, subscriber->topic); - uint32_t skip = subscriber->skip; - send(sock, (char*)&skip, 4, 0); - send(sock, buffer, length - 4, 0);*/ - - // add the socket to the list of connections - impl->num_connections++; - struct ps_tcp_transport_connection* old_connections = impl->connections; - impl->connections = (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * impl->num_connections); - for (int i = 0; i < impl->num_connections - 1; i++) - { - impl->connections[i] = old_connections[i]; - } - - struct ps_tcp_transport_connection* new_connection = &impl->connections[impl->num_connections - 1]; - new_connection->socket = sock; - new_connection->endpoint = *ep; - new_connection->waiting_for_header = true; - new_connection->subscriber = subscriber; - new_connection->connecting = true; - - ps_event_set_add_socket(&subscriber->node->events, sock); - - if (impl->num_connections - 1) - { - free(old_connections); - } +#ifdef __cplusplus } - -void ps_tcp_transport_unsubscribe(struct ps_transport_t* transport, struct ps_sub_t* subscriber) -{ - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; - - // remove all transports which reference this subscriber - int num_to_remove = 0; - for (int i = 0; i < impl->num_connections; i++) - { - if (impl->connections[i].subscriber == subscriber) - { - num_to_remove++; - } - } - -#ifdef PUBSUB_VERBOSE - printf("Removing %i tcp subs\n", num_to_remove); -#endif - - if (num_to_remove > 0) - { - // Free our subscribers and any buffers - int iter = 0; - int new_size = impl->num_connections - num_to_remove; - struct ps_tcp_transport_connection* new_connections = new_size == 0 ? 0 : (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * new_size); - for (int i = 0; i < impl->num_connections; i++) - { - if (impl->connections[i].subscriber != subscriber) - { - new_connections[iter++] = impl->connections[i]; - continue; - } - - if (!impl->connections[i].waiting_for_header) - { - struct ps_sub_t* sub = impl->connections[i].subscriber; - sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); - } - ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); -#ifdef _WIN32 - closesocket(impl->connections[i].socket); -#else - close(impl->connections[i].socket); #endif - } - impl->num_connections = new_size; - free(impl->connections); - impl->connections = new_connections; - } -} - - -void ps_tcp_transport_destroy(struct ps_transport_t* transport) -{ - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; - - // Free our subscribers and any buffers - for (int i = 0; i < impl->num_connections; i++) - { - if (!impl->connections[i].waiting_for_header) - { - struct ps_sub_t* sub = impl->connections[i].subscriber; - sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); - } - ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); -#ifdef _WIN32 - closesocket(impl->connections[i].socket); -#else - close(impl->connections[i].socket); -#endif - } - - for (int i = 0; i < impl->num_clients; i++) - { - ps_event_set_remove_socket(&impl->node->events, impl->clients[i].socket); -#ifdef _WIN32 - closesocket(impl->clients[i].socket); -#else - close(impl->clients[i].socket); -#endif - } - -#ifdef _WIN32 - closesocket(impl->socket); -#else - close(impl->socket); -#endif - - if (impl->num_clients) - { - free(impl->clients); - } - - if (impl->num_connections) - { - free(impl->connections); - } - - free(impl); -} - -void ps_tcp_transport_init(struct ps_transport_t* transport, struct ps_node_t* node) -{ -#ifdef __unix__ - signal(SIGPIPE, SIG_IGN); -#endif - - transport->spin = ps_tcp_transport_spin; - transport->subscribe = ps_tcp_transport_subscribe; - transport->unsubscribe = ps_tcp_transport_unsubscribe; - transport->destroy = ps_tcp_transport_destroy; - transport->pub = ps_tcp_transport_pub; - transport->uuid = 1; - - struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)malloc(sizeof(struct ps_tcp_transport_impl)); - - impl->num_clients = 0; - impl->num_connections = 0; - - impl->node = node; - - impl->socket = socket(AF_INET, SOCK_STREAM, 0); - - struct sockaddr_in server_addr; - server_addr.sin_family = AF_INET; - server_addr.sin_addr.s_addr = INADDR_ANY; - server_addr.sin_port = 0;// we want an ephemeral port - if (bind(impl->socket, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) - { - ps_print_socket_error("error binding tcp transport socket"); - } - - socklen_t outlen = sizeof(struct sockaddr_in); - struct sockaddr_in outaddr; - getsockname(impl->socket, (struct sockaddr*)&outaddr, &outlen); - transport->transport_info = ntohs(outaddr.sin_port); - - printf("Bound tcp to %i\n", transport->transport_info); - - // set non-blocking -#ifdef _WIN32 - DWORD nonBlocking = 1; - if (ioctlsocket(impl->socket, FIONBIO, &nonBlocking) != 0) - { - ps_print_socket_error("Failed to Set Socket as Non-Blocking"); - closesocket(impl->socket); - return; - } -#endif -#ifdef ARDUINO - fcntl(impl->socket, F_SETFL, O_NONBLOCK); -#endif -#ifdef __unix__ - int flags = fcntl(impl->socket, F_GETFL); - fcntl(impl->socket, F_SETFL, flags | O_NONBLOCK); -#endif - - listen(impl->socket, 5); - - ps_event_set_add_socket(&node->events, impl->socket); - - transport->impl = (void*)impl; -} #endif diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index 9356184..2b78652 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -26,16 +27,16 @@ namespace pubsub { -static std::map _remappings; +extern std::map _remappings; // how intraprocess passing works class SubscriberBase; class PublisherBase; // this mutex protects both of the below -static std::mutex _publisher_mutex; -static std::multimap _publishers; -static std::multimap _subscribers; +extern std::mutex _publisher_mutex; +extern std::multimap _publishers; +extern std::multimap _subscribers; // this assumes topic and ns are properly checked // ns should not have a leading slash, topic should if it is absolute @@ -163,12 +164,74 @@ inline std::string validate_name(const std::string& name, bool remove_leading_sl return name; } +struct ParameterContainer +{ + std::mutex lock; + double d; + float f; + int i; + std::string s; + + operator double() const + { + return d; + } + + operator float() const + { + return f; + } + + operator int() const + { + return i; + } + + operator std::string() const + { + return s; + } +}; + +class Node; +template +class Parameter +{ + friend class Node; + std::shared_ptr value; +public: + + operator T() const + { + std::lock_guard lock(value->lock); + return (T)*value; + } + + T get() const + { + std::lock_guard lock(value->lock); + return (T)*value; + } + + /*void operator=(const T& new_value) + { + // todo update parameter value + value->d = new_value; + }*/ +}; + // todo need to make sure multiple subscribers in a process share data // safe to use each node in a different thread after initialize is called // making calls to functions on the same node is not thread safe +typedef std::unique_ptr SubscriberPtr; +typedef std::unique_ptr PublisherPtr; class SubscriberBase; class Spinner; +template +class Subscriber; +template +class Publisher; class Node { friend class Spinner; @@ -210,6 +273,10 @@ class Node ~Node() { + if (params_data_) + { + ps_destroy_parameters(params_data_.get()); + } ps_node_destroy(&node_); } @@ -254,6 +321,54 @@ class Node { return event_set_; } + + template + SubscriberBase* subscribe(const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = -1, int skip = 0) + { + return new Subscriber(*this, topic, cb, queue_size, preferred_transport, skip); + } + + template + Publisher* advertise(const std::string& topic, bool latched = false, int preferred_transport = -1) + { + return new Publisher(*this, topic, latched, preferred_transport); + } + + std::unique_ptr params_data_; + std::map> params_; + + Parameter parameter(const std::string& name, double default_value, const std::string& desc = "", + double min = -10000, double max = 10000) + { + if (!params_data_) + { + params_data_.reset(new ps_parameters); + ps_create_parameters(getNode(), params_data_.get(), [](const char* name, double value, void* data) + { + auto tthis = (Node*)data; + auto res = tthis->params_.find(name); + if (res == tthis->params_.end()) + { + return; + } + + auto shrd = res->second.lock(); + if (shrd) + { + std::lock_guard lock(shrd->lock); + shrd->d = value; + } + }, this); + } + + ps_add_parameter_double(params_data_.get(), name.c_str(), desc.c_str(), default_value, min, max); + + Parameter p; + p.value = std::make_shared(); + p.value->d = default_value; + params_[name] = p.value; + return p; + } // mark that we have a message to process void mark() { marked_ = true; } @@ -277,6 +392,9 @@ class PublisherBase std::vector subs_; public: + + virtual ~PublisherBase() {} + const std::string& GetTopic() { return remapped_topic_; @@ -288,14 +406,14 @@ class PublisherBase } }; -template -class Subscriber; template class Publisher: public PublisherBase { std::shared_ptr latched_msg_; public: friend class Subscriber; + + typedef std::unique_ptr> Ptr; Publisher(Node& node, const std::string& topic, bool latched = false, int preferred_transport = 0)// : topic_(topic) { @@ -375,7 +493,7 @@ class Publisher: public PublisherBase // now go through my local subscriber list for (auto& sub: subs_) { - //printf("Publishing locally with no copy..\n"); + printf("Publishing locally with no copy..\n"); auto specific_sub = (Subscriber*)sub; ps_event_set_trigger(specific_sub->node_->getEventSet()); @@ -409,7 +527,7 @@ class Publisher: public PublisherBase // now go through my local subscriber list for (auto& sub: subs_) { - //printf("Publishing locally with a copy..\n"); + printf("Publishing locally with a copy..\n"); if (!copy) { //copy to shared ptr @@ -523,6 +641,8 @@ class SubscriberBase } public: + virtual ~SubscriberBase() {} + ps_sub_t* GetSub() { return &subscriber_; @@ -547,6 +667,8 @@ class Subscriber: public SubscriberBase public: + typedef std::unique_ptr> Ptr; + Subscriber(Node& node, const std::string& topic, std::function&)> cb, unsigned int queue_size = 1, int preferred_transport = -1, int skip = 0) : cb_(cb), queue_size_(queue_size) { node_ = &node; @@ -615,6 +737,34 @@ class Subscriber: public SubscriberBase return false; } + std::shared_ptr PopOne() + { + queue_mutex_.lock(); + if (!queue_.size()) + { + queue_mutex_.unlock(); + return {}; + } + auto back = queue_.back(); + queue_.pop_back(); + queue_mutex_.unlock(); + return back; + } + + void PushOne(const std::shared_ptr& msg) + { + auto specific_sub = this; + //ps_event_set_trigger(specific_sub->node_->getEventSet()); + specific_sub->queue_mutex_.lock(); + specific_sub->queue_.push_front(msg); + if (specific_sub->queue_.size() > specific_sub->queue_size_) + { + specific_sub->queue_.pop_back(); + } + specific_sub->queue_mutex_.unlock(); + //specific_sub->node_->mark(); + } + ~Subscriber() { close(); diff --git a/include/pubsub_cpp/Time.h b/include/pubsub_cpp/Time.h index 61783bf..8ba695a 100644 --- a/include/pubsub_cpp/Time.h +++ b/include/pubsub_cpp/Time.h @@ -38,6 +38,16 @@ class Duration } + bool operator==(const Duration& rhs) const + { + return this->usec == rhs.usec; + } + + bool operator!=(const Duration& rhs) const + { + return this->usec != rhs.usec; + } + bool operator<(const Duration& rhs) const { return this->usec < rhs.usec; @@ -47,6 +57,19 @@ class Duration { return this->usec > rhs.usec; } + + Duration operator+(const Duration& rhs) const + { + Duration out; + out.usec = this->usec + rhs.usec; + return out; + } + + Duration& operator+=(const Duration& rhs) + { + this->usec += rhs.usec; + return *this; + } double toSec() const { @@ -83,6 +106,16 @@ class Time out.usec = this->usec - rhs.usec; return out; } + + bool operator==(const Time& rhs) const + { + return this->usec == rhs.usec; + } + + bool operator!=(const Time& rhs) const + { + return this->usec != rhs.usec; + } bool operator<(const Time& rhs) const { @@ -111,6 +144,12 @@ class Time return out; } + Time& operator+=(const Duration& rhs) + { + this->usec += rhs.usec; + return *this; + } + static Time now() { #ifdef _WIN32 diff --git a/src/TCPTransport.c b/src/TCPTransport.c new file mode 100644 index 0000000..910125c --- /dev/null +++ b/src/TCPTransport.c @@ -0,0 +1,885 @@ +// Must be included first on windows +#include + +#include +#include +#include +#include +#include +//#include + +#include +#include +#include + +#ifdef __unix__ +#include +#endif + +void remove_client_socket(struct ps_tcp_transport_impl* transport, int socket, struct ps_node_t* node) +{ + // find the index + int i = 0; + for (; i < transport->num_clients; i++) + { + if (transport->clients[i].socket == socket)// socket packed in address + { + break; + } + } + +#ifdef _WIN32 + closesocket(socket); +#else + close(socket); +#endif + + if (transport->clients[i].packet_data) + { + free(transport->clients[i].packet_data); + } + + if (transport->clients[i].queued_message) + { + ps_msg_ref_free(transport->clients[i].queued_message, transport->clients[i].publisher->allocator); + } + + // free queued messages + if (transport->clients[i].num_queued_messages) + { + for (int j = 0; j < transport->clients[i].num_queued_messages; j++) + { + ps_msg_ref_free(transport->clients[i].queued_messages[j].msg, transport->clients[i].publisher->allocator); + } + free(transport->clients[i].queued_messages); + } + + struct ps_tcp_client_t* old_clients = transport->clients; + transport->num_clients -= 1; + + // close the socket and dont wait on it anymore + ps_event_set_remove_socket(&node->events, transport->clients[i].socket); + + if (transport->num_clients) + { + transport->clients = (struct ps_tcp_client_t*)malloc(sizeof(struct ps_tcp_client_t) * transport->num_clients); + for (int j = 0; j < i; j++) + { + transport->clients[j] = old_clients[j]; + } + + for (int j = i + 1; j <= transport->num_clients; j++) + { + transport->clients[j - 1] = old_clients[j]; + } + } + free(old_clients); +} + +void ps_tcp_remove_connection(struct ps_tcp_transport_impl* impl, int index) +{ + // Free our subscribers and any buffers + int iter = 0; + int new_size = impl->num_connections - 1; + struct ps_tcp_transport_connection* new_connections = new_size == 0 ? 0 : (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * new_size); + for (int i = 0; i < impl->num_connections; i++) + { + if (i != index) + { + new_connections[iter++] = impl->connections[i]; + continue; + } + + if (!impl->connections[i].waiting_for_header) + { + free(impl->connections[i].packet_data); + } + ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); +#ifdef _WIN32 + closesocket(impl->connections[i].socket); +#else + close(impl->connections[i].socket); +#endif + } + impl->num_connections = new_size; + free(impl->connections); + impl->connections = new_connections; +} + +int ps_tcp_transport_spin(struct ps_transport_t* transport, struct ps_node_t* node) +{ + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; + int socket = accept(impl->socket, 0, 0); + if (socket > 0) + { +#ifdef PUBSUB_VERBOSE + printf("Got new socket connection!\n"); +#endif + + // add it to the list yo + impl->num_clients++; + struct ps_tcp_client_t* old_sockets = impl->clients; + impl->clients = (struct ps_tcp_client_t*)malloc(sizeof(struct ps_tcp_client_t) * impl->num_clients); + for (int i = 0; i < impl->num_clients - 1; i++) + { + impl->clients[i] = old_sockets[i]; + } + struct ps_tcp_client_t* new_client = &impl->clients[impl->num_clients - 1]; + new_client->socket = socket; + new_client->needs_removal = false; + new_client->current_packet_size = 0; + new_client->desired_packet_size = 0; + new_client->packet_data = 0; + new_client->queued_message = 0; + new_client->queued_message_length = 0; + new_client->queued_message_written = 0; + new_client->queued_messages = 0; + new_client->num_queued_messages = 0; + + // set non-blocking +#ifdef _WIN32 + DWORD nonBlocking = 1; + if (ioctlsocket(socket, FIONBIO, &nonBlocking) != 0) + { + printf("Failed to Set Socket as Non-Blocking!\n"); + closesocket(socket); + return 0; + } +#endif +#ifdef ARDUINO + fcntl(socket, F_SETFL, O_NONBLOCK); +#endif +#ifdef __unix__ + int flags = fcntl(socket, F_GETFL); + fcntl(socket, F_SETFL, flags | O_NONBLOCK); +#endif + + ps_event_set_add_socket(&node->events, socket); + + if (impl->num_clients - 1) + { + free(old_sockets); + } + } + //printf("polled\n"); + +// remove any old sockets + for (int i = 0; i < impl->num_clients; i++) + { + if (impl->clients[i].needs_removal) + { + // add it to the list + struct ps_client_t client; + client.endpoint.address = impl->clients[i].socket; + client.endpoint.port = 255;// p->port; + client.stream_id = 0; + ps_pub_remove_client(impl->clients[i].publisher, &client);// todo this is probably unsafe.... + + remove_client_socket(impl, impl->clients[i].socket, impl->clients[i].publisher->node); + + i = i - 1; + break; + } + } + + // update our sockets yo + for (int i = 0; i < impl->num_clients; i++) + { + struct ps_tcp_client_t* client = &impl->clients[i]; + + // send queued messages until we block or cant send anymore + while (client->queued_message != 0) + { + int to_send = client->queued_message_length - client->queued_message_written; + char* data = (char*)client->queued_message->data; + int sent = send(client->socket, &data[client->queued_message_written], to_send, 0); + if (sent > 0) + { + client->queued_message_written += sent; + } +#ifdef WIN32 + else if (sent < 0 && WSAGetLastError() != WSAEWOULDBLOCK) +#else + else if (sent < 0 && errno != EAGAIN) +#endif + { + //printf("needs removal %i\n", errno); + client->needs_removal = true; + } + + //printf("Sending more: %i to make %i of %i\n", sent, client->queued_message_written, client->queued_message_length); + + if (client->queued_message_written == client->queued_message_length) + { + //printf("Message sent.\n"); + ps_msg_ref_free(client->queued_message, client->publisher->allocator); + client->queued_message = 0; + + // we finished! check if there are more to send + if (client->num_queued_messages > 0) + { + // grab a message from the front of our message queue + client->queued_message = client->queued_messages[0].msg; + client->queued_message_written = 0; + client->queued_message_length = client->queued_messages[0].msg->len + sizeof(struct ps_msg_header); + + client->num_queued_messages -= 1; + if (client->num_queued_messages == 0) + { + free(client->queued_messages); + client->queued_messages = 0; + continue; + } + + struct ps_tcp_client_queued_message_t* msgs = (struct ps_tcp_client_queued_message_t*)malloc(client->num_queued_messages * sizeof(struct ps_tcp_client_queued_message_t)); + for (int i = 0; i < client->num_queued_messages; i++) + { + msgs[i] = client->queued_messages[i + 1];// take from the front + } + free(client->queued_messages); + client->queued_messages = msgs; + + // continue so we can attempt to send again + } + else + { + ps_event_set_remove_socket_write(&node->events, client->socket); + break;// no more to send + } + } + else + { + break;// we couldnt send anymore atm + } + } + + // check for new data and add it to the packet if present + char buf[1500]; + // if we havent gotten a header yet, just check for that + if (client->desired_packet_size == 0) + { + const int header_size = sizeof(struct ps_msg_header); + int len = recv(client->socket, buf, header_size, MSG_PEEK); + //printf("peek %i desired size %i\n", len, header_size); + if (len == 0) + { + client->needs_removal = true; + continue; + } + if (len < header_size) + { + continue;// no header yet + } + + char message_type = buf[0];// not used atm + + // we actually got the header! start looking for the message + len = recv(client->socket, buf, header_size, 0); + //connection->packet_type = message_type; + //printf("recv %i from client->socket desired size 0 2\n", len); + //client->waiting_for_header = false; + client->desired_packet_size = *(uint32_t*)&buf[1]; + //printf("Incoming message with %i bytes\n", client->desired_packet_size); + client->packet_data = (char*)malloc(client->desired_packet_size); + + client->current_packet_size = 0; + } + // read in the message + if (client->desired_packet_size != 0) + { + int remaining_size = client->desired_packet_size - client->current_packet_size; + // check for new messages and read until we hit packet size + int len = recv(client->socket, &client->packet_data[client->current_packet_size], remaining_size, 0); + //printf("recv %i from client->socket\n", len); + if (len > 0) + { + //printf("Read %i bytes of message\n", len); + client->current_packet_size += len; + + if (client->current_packet_size == client->desired_packet_size) + { +#ifdef PUBSUB_VERBOSE + printf("message finished\n"); +#endif + + if (true)// todo look at message id + { + // its a subscribe + const char* topic = &client->packet_data[4]; + // check if this matches any of our publishers + for (unsigned int pi = 0; pi < node->num_pubs; pi++) + { + struct ps_pub_t* pub = node->pubs[pi]; + if (strcmp(topic, pub->topic) == 0) + { + uint32_t skip = *(uint32_t*)&client->packet_data[0]; + // send response and start publishing + struct ps_client_t sub_client; + sub_client.endpoint.address = client->socket; + sub_client.endpoint.port = 255;// p->port; + sub_client.last_keepalive = 10000000000000;//GetTickCount64();// use the current time stamp + sub_client.sequence_number = 0; + sub_client.stream_id = 0; + sub_client.modulo = skip > 0 ? skip + 1 : 0; + sub_client.transport = transport; + + impl->clients[i].publisher = pub; + + // send the client the acknowledgement and message definition + char buf[1500]; + int32_t length = ps_serialize_message_definition((void*)buf, pub->message_definition); + struct ps_msg_header hdr; + hdr.pid = PS_TCP_PROTOCOL_MESSAGE_DEFINITION;// message definition + hdr.length = length; + hdr.id = hdr.seq = 0; + send(impl->clients[i].socket, (char*)&hdr, sizeof(hdr), 0); + send(impl->clients[i].socket, buf, length, 0); + +#ifdef PUBSUB_VERBOSE + printf("TCPTransport: Got subscribe request, adding client if we haven't already\n"); +#endif + ps_pub_add_client(pub, &sub_client); + + break; + } + } + } + + free(client->packet_data); + client->packet_data = 0; + client->desired_packet_size = 0; + } + } + } + } + + int message_count = 0; + for (int i = 0; i < impl->num_connections; i++) + { + struct ps_tcp_transport_connection* connection = &impl->connections[i]; + char buf[1500]; + if (connection->connecting) + { + //printf("checking for connected\n"); + // select to check for writability + fd_set wfds; + struct timeval tv; + int retval; + + FD_ZERO(&wfds); + FD_SET(connection->socket, &wfds); + + tv.tv_sec = 0; + tv.tv_usec = 0; + retval = select(connection->socket + 1, NULL, &wfds, NULL, &tv); + //printf("select\n"); + if (retval == -1) + { + // error? + printf("socket errored while connecting\n"); + } + else if (retval) + { + // socket is writable + //printf("socket writable\n"); + + // make the subscribe request in a "packet" + // a packet is an int length followed by data + int32_t length = strlen(connection->subscriber->topic) + 1 + 4; + + struct ps_msg_header hdr; + hdr.pid = 0x01; + hdr.length = length; + hdr.id = hdr.seq = 0; + send(connection->socket, (char*)&hdr, sizeof(hdr), 0); + + // make the request + uint32_t skip = connection->subscriber->skip; + send(connection->socket, (char*)&skip, 4, 0); + send(connection->socket, connection->subscriber->topic, length - 4, 0); + + connection->connecting = false; + } + } + // if we havent gotten a header yet, just check for that + else if (connection->waiting_for_header) + { + const int header_size = sizeof(struct ps_msg_header); + int len = recv(connection->socket, buf, header_size, MSG_PEEK); + //printf("peek got: %i\n", len); + if (len == 0) + { + // we got disconnected + ps_tcp_remove_connection(impl, i); + i--; + continue; + } + else if (len < header_size) + { + continue;// no header yet + } + + char message_type = buf[0]; + + // we actually got the header! start looking for the message + len = recv(connection->socket, buf, header_size, 0); + connection->packet_type = message_type; + connection->waiting_for_header = false; + connection->packet_size = *(uint32_t*)&buf[1]; + //printf("Incoming message with %i bytes\n", impl->connections[i].packet_size); + connection->packet_data = (char*)connection->subscriber->allocator->alloc(connection->packet_size, connection->subscriber->allocator->context); + + connection->current_size = 0; + } + else // read in the message + { + int remaining_size = connection->packet_size - connection->current_size; + + // check for new messages and read until we hit packet size + int len = recv(connection->socket, &connection->packet_data[connection->current_size], remaining_size, 0); + //printf("len %i\n", len); + if (len == 0) + { + // we got disconnected + ps_tcp_remove_connection(impl, i); + i--; + continue; + } + else if (len > 0) + { + connection->current_size += len; + //printf("Read %i bytes of message, so far: %i\n", len, connection->current_size); + + if (connection->current_size == connection->packet_size) + { + //printf("message finished type %x\n", connection->packet_type); + if (connection->packet_type == PS_TCP_PROTOCOL_MESSAGE_DEFINITION) + { + //printf("Was message definition\n"); + if (connection->subscriber->type == 0) + { + // todo put this in a function so we cant accidentally forget it + if (connection->subscriber->received_message_def.fields == 0) + { + ps_deserialize_message_definition(connection->packet_data, &connection->subscriber->received_message_def); + } + + // call the callback as well + if (node->def_cb) + { + node->def_cb(&connection->subscriber->received_message_def, node->def_cb_data); + } + } + + connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); + } + else if (connection->packet_type == PS_TCP_PROTOCOL_DATA) + { + //printf("added to queue\n"); + // decode and add it to the queue + struct ps_msg_info_t message_info; + message_info.address = connection->endpoint.address; + message_info.port = connection->endpoint.port; + + ps_sub_receive(connection->subscriber, connection->packet_data, connection->packet_size, false, &message_info); + + // remove the reference to packet data so we don't try and double free it on destroy + connection->packet_data = 0; + message_count++; + } + else + { + // unhandled packet id + connection->subscriber->allocator->free(connection->packet_data, connection->subscriber->allocator->context); + } + connection->waiting_for_header = true; + } + } + } + } + return message_count; +} + +void ps_tcp_transport_pub(struct ps_transport_t* transport, struct ps_pub_t* publisher, struct ps_client_t* client, struct ps_msg_ref_t* msg) +{ + // todo dont + int length = msg->len; + void* message = msg->data; + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; + + // the client packs the socket id in the addr + int socket = client->endpoint.address; + + // okay, so new version, if any write fails (EAGAIN or < expected size) + // we make a copy of the entire message and store it on that client to try and send again in our update loop + // if we get into this function and this client already has a queued message, just drop this one + struct ps_tcp_client_t* tclient = 0; + for (int i = 0; i < impl->num_clients; i++) + { + if (impl->clients[i].socket == socket) + { + tclient = &impl->clients[i]; + break; + } + } + + if (tclient->queued_message != 0) + { + // check if we have queue space left + + // for now hardcode max queue size + const int max_queue_size = 10; + + // add a reference to the message and queue it up + ps_msg_ref_add(msg); + + // this if statement is unnecessary, but I added it for the sake of testing/completeness + if (tclient->queued_message == 0) + { + tclient->queued_message = msg; + tclient->queued_message_length = length + sizeof(struct ps_msg_header); + tclient->queued_message_written = 0; + } + else if (tclient->num_queued_messages >= max_queue_size) + { + // todo use a deque lol + // swap everything down, freeing the first + for (int i = tclient->num_queued_messages - 1; i >= 1; i--) + { + tclient->queued_messages[i] = tclient->queued_messages[i - 1]; + } + tclient->queued_messages[0].msg = msg; + printf("dropped message on topic '%s'\n", publisher->topic); + return;// drop it, we are out of queue space + } + else + { + //printf("queuing up message %i on topic '%s'\n", tclient->num_queued_messages, publisher->topic); + + // add the message to the front of the queue + tclient->num_queued_messages += 1; + struct ps_tcp_client_queued_message_t* msgs = (struct ps_tcp_client_queued_message_t*)malloc(tclient->num_queued_messages * sizeof(struct ps_tcp_client_queued_message_t)); + + msgs[0].msg = msg; + for (int i = 0; i < tclient->num_queued_messages - 1; i++) + { + msgs[i + 1] = tclient->queued_messages[i]; + } + free(tclient->queued_messages); + tclient->queued_messages = msgs; + + return; + } + } + //printf("started writing\n"); + // try and write, if any of these fail, make a copy + + // the message header is already filled out with the packet id and length + + int32_t desired_len = sizeof(struct ps_msg_header) + length; + //printf("trying to send message of %i bytes\n", desired_len); + int32_t c = send(socket, (char*)message, desired_len, 0); + if (c < desired_len && c >= 0) + { + tclient->queued_message_written = c; + goto FAILCOPY; + } + if (c < 0) + { +#ifdef WIN32 + int error = WSAGetLastError(); + if (error == WSAEWOULDBLOCK) +#else + if (errno == EAGAIN || errno == EWOULDBLOCK) +#endif + { + tclient->queued_message_written = 0; + goto FAILCOPY; + } + goto FAILDISCONNECT; + } + + //printf("wrote all\n"); + return; + + char* data; +FAILDISCONNECT: + //printf("Disconnected: %s\n", strerror(err)); + tclient->needs_removal = true; + return; + +FAILCOPY: + // add a reference count and put it in our queue + ps_msg_ref_add(msg); + + //printf("Wrote %i bytes\n", tclient->queued_message_written); + + tclient->queued_message = msg; + tclient->queued_message_length = length + sizeof(struct ps_msg_header); + ps_event_set_add_socket_write(&publisher->node->events, socket); + return; +} + +void ps_tcp_transport_subscribe(struct ps_transport_t* transport, struct ps_sub_t* subscriber, struct ps_endpoint_t* ep, uint32_t transport_info) +{ + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; + + // check if we already have a sub for this subscriber with this endpoint + // if so, ignore it + for (int i = 0; i < impl->num_connections; i++) + { + if (impl->connections[i].endpoint.port == ep->port && + impl->connections[i].endpoint.address == ep->address && + impl->connections[i].subscriber->sub_id == subscriber->sub_id) + { + return; + } + } + +#ifdef _WIN32 + SOCKET sock = socket(AF_INET, SOCK_STREAM, 0); +#else + int sock = socket(AF_INET, SOCK_STREAM, 0); +#endif + + // set non-blocking +#ifdef _WIN32 + DWORD nonBlocking = 1; + if (ioctlsocket(sock, FIONBIO, &nonBlocking) != 0) + { + ps_print_socket_error("Failed to Set Socket as Non-Blocking"); + closesocket(sock); + return; + } +#endif +#ifdef ARDUINO + fcntl(sock, F_SETFL, O_NONBLOCK); +#endif +#ifdef __unix__ + int flags = fcntl(sock, F_GETFL); + fcntl(sock, F_SETFL, flags | O_NONBLOCK); +#endif + + // Actually connect + //printf("connecting\n"); + struct sockaddr_in server_addr; + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = htonl(ep->address); + server_addr.sin_port = htons(transport_info); + int connect_result = connect(sock, (struct sockaddr*)&server_addr, sizeof(server_addr)); + if (connect_result != 0) + { +#ifdef _WIN32 + if (WSAGetLastError() != WSAEWOULDBLOCK)//WSAEINPROGRESS +#else + if (errno != EINPROGRESS) +#endif + { + ps_print_socket_error("error connecting tcp socket"); + return; + } + } + + //printf("%i %i %i %i\n", (ep->address & 0xFF000000) >> 24, (ep->address & 0xFF0000) >> 16, (ep->address & 0xFF00) >> 8, (ep->address & 0xFF)); + + // make the subscribe request in a "packet" + // a packet is an int length followed by data + /*int8_t packet_type = 0x01;//subscribe + send(sock, (char*)&packet_type, 1, 0); + + int32_t length = strlen(subscriber->topic) + 1 + 4; + send(sock, (char*)&length, 4, 0); + + // make the request + char buffer[500]; + strcpy(buffer, subscriber->topic); + uint32_t skip = subscriber->skip; + send(sock, (char*)&skip, 4, 0); + send(sock, buffer, length - 4, 0);*/ + + // add the socket to the list of connections + impl->num_connections++; + struct ps_tcp_transport_connection* old_connections = impl->connections; + impl->connections = (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * impl->num_connections); + for (int i = 0; i < impl->num_connections - 1; i++) + { + impl->connections[i] = old_connections[i]; + } + + struct ps_tcp_transport_connection* new_connection = &impl->connections[impl->num_connections - 1]; + new_connection->socket = sock; + new_connection->endpoint = *ep; + new_connection->waiting_for_header = true; + new_connection->subscriber = subscriber; + new_connection->connecting = true; + + ps_event_set_add_socket(&subscriber->node->events, sock); + + if (impl->num_connections - 1) + { + free(old_connections); + } +} + +void ps_tcp_transport_unsubscribe(struct ps_transport_t* transport, struct ps_sub_t* subscriber) +{ + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; + + // remove all transports which reference this subscriber + int num_to_remove = 0; + for (int i = 0; i < impl->num_connections; i++) + { + if (impl->connections[i].subscriber == subscriber) + { + num_to_remove++; + } + } + +#ifdef PUBSUB_VERBOSE + printf("Removing %i tcp subs\n", num_to_remove); +#endif + + if (num_to_remove > 0) + { + // Free our subscribers and any buffers + int iter = 0; + int new_size = impl->num_connections - num_to_remove; + struct ps_tcp_transport_connection* new_connections = new_size == 0 ? 0 : (struct ps_tcp_transport_connection*)malloc(sizeof(struct ps_tcp_transport_connection) * new_size); + for (int i = 0; i < impl->num_connections; i++) + { + if (impl->connections[i].subscriber != subscriber) + { + new_connections[iter++] = impl->connections[i]; + continue; + } + + if (!impl->connections[i].waiting_for_header) + { + struct ps_sub_t* sub = impl->connections[i].subscriber; + sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); + } + ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); +#ifdef _WIN32 + closesocket(impl->connections[i].socket); +#else + close(impl->connections[i].socket); +#endif + } + impl->num_connections = new_size; + free(impl->connections); + impl->connections = new_connections; + } +} + + +void ps_tcp_transport_destroy(struct ps_transport_t* transport) +{ + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)transport->impl; + + // Free our subscribers and any buffers + for (int i = 0; i < impl->num_connections; i++) + { + if (!impl->connections[i].waiting_for_header) + { + struct ps_sub_t* sub = impl->connections[i].subscriber; + sub->allocator->free(impl->connections[i].packet_data, sub->allocator->context); + } + ps_event_set_remove_socket(&impl->node->events, impl->connections[i].socket); +#ifdef _WIN32 + closesocket(impl->connections[i].socket); +#else + close(impl->connections[i].socket); +#endif + } + + for (int i = 0; i < impl->num_clients; i++) + { + ps_event_set_remove_socket(&impl->node->events, impl->clients[i].socket); +#ifdef _WIN32 + closesocket(impl->clients[i].socket); +#else + close(impl->clients[i].socket); +#endif + } + +#ifdef _WIN32 + closesocket(impl->socket); +#else + close(impl->socket); +#endif + + if (impl->num_clients) + { + free(impl->clients); + } + + if (impl->num_connections) + { + free(impl->connections); + } + + free(impl); +} + +void ps_tcp_transport_init(struct ps_transport_t* transport, struct ps_node_t* node) +{ +#ifdef __unix__ + signal(SIGPIPE, SIG_IGN); +#endif + + transport->spin = ps_tcp_transport_spin; + transport->subscribe = ps_tcp_transport_subscribe; + transport->unsubscribe = ps_tcp_transport_unsubscribe; + transport->destroy = ps_tcp_transport_destroy; + transport->pub = ps_tcp_transport_pub; + transport->uuid = 1; + + struct ps_tcp_transport_impl* impl = (struct ps_tcp_transport_impl*)malloc(sizeof(struct ps_tcp_transport_impl)); + + impl->num_clients = 0; + impl->num_connections = 0; + + impl->node = node; + + impl->socket = socket(AF_INET, SOCK_STREAM, 0); + + struct sockaddr_in server_addr; + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = INADDR_ANY; + server_addr.sin_port = 0;// we want an ephemeral port + if (bind(impl->socket, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) + { + ps_print_socket_error("error binding tcp transport socket"); + } + + socklen_t outlen = sizeof(struct sockaddr_in); + struct sockaddr_in outaddr; + getsockname(impl->socket, (struct sockaddr*)&outaddr, &outlen); + transport->transport_info = ntohs(outaddr.sin_port); + + printf("Bound tcp to %i\n", transport->transport_info); + + // set non-blocking +#ifdef _WIN32 + DWORD nonBlocking = 1; + if (ioctlsocket(impl->socket, FIONBIO, &nonBlocking) != 0) + { + ps_print_socket_error("Failed to Set Socket as Non-Blocking"); + closesocket(impl->socket); + return; + } +#endif +#ifdef ARDUINO + fcntl(impl->socket, F_SETFL, O_NONBLOCK); +#endif +#ifdef __unix__ + int flags = fcntl(impl->socket, F_GETFL); + fcntl(impl->socket, F_SETFL, flags | O_NONBLOCK); +#endif + + listen(impl->socket, 5); + + ps_event_set_add_socket(&node->events, impl->socket); + + transport->impl = (void*)impl; +} diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 2e69dab..68c64fd 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -6,8 +6,8 @@ foreach(test_file ${tests}) message(STATUS "create ${test_file} executable from source: ${test_source}") add_executable("${test_file}" "${test_source}") target_include_directories("${test_file}" PUBLIC ../include) - add_dependencies("${test_file}" pubsub pubsub_msgs) - target_link_libraries("${test_file}" pubsub ${CMAKE_THREAD_LIBS_INIT} pubsub_msgs pubsub_test_msgs) + add_dependencies("${test_file}" pubsub pubsub_msgs pubsub_cpp) + target_link_libraries("${test_file}" pubsub ${CMAKE_THREAD_LIBS_INIT} pubsub_msgs pubsub_test_msgs pubsub_cpp) # Auto populate the tests from test source file # Note : CMake must be reconfigured if tests are added/renamed/removed from test source file From de8f7d52c7d2f2ae71da4acc68d040c84909b39c Mon Sep 17 00:00:00 2001 From: Matthew B Date: Tue, 19 Aug 2025 22:06:36 -0700 Subject: [PATCH 34/34] Fixes --- examples/simple_pub.cpp | 195 ++++++-------------------------------- include/pubsub_cpp/Node.h | 5 + src/Publisher.c | 2 +- 3 files changed, 33 insertions(+), 169 deletions(-) diff --git a/examples/simple_pub.cpp b/examples/simple_pub.cpp index fe131ea..6bdc61a 100644 --- a/examples/simple_pub.cpp +++ b/examples/simple_pub.cpp @@ -7,185 +7,44 @@ #include #include -#include -#include -#include -#include -#include -#include #include int main() { - pubsub::Node node("simple_publisher"); - - struct ps_transport_t tcp_transport; - ps_tcp_transport_init(&tcp_transport, node.getNode()); - ps_node_add_transport(node.getNode(), &tcp_transport); + // Create the node + pubsub::Node node("simple_publisher"/*node name*/); - pubsub::Publisher string_pub(node, "/data"); + // Adds TCP transport (optional) + struct ps_transport_t tcp_transport; + ps_tcp_transport_init(&tcp_transport, node.getNode()); + ps_node_add_transport(node.getNode(), &tcp_transport); - //string_pub.addCustomEndpoint(0x7FFFFFFF, 11312, 0); + // Create the publisher + pubsub::Publisher string_pub(node, "/data"/*topic name*/, + false/*true to "latch" the topic*/); - pubsub::Publisher image_pub(node, "/image"); - - - pubsub::Publisher image_pub2(node, "/image2"); - - pubsub::Publisher costmap_pub(node, "/costmap"); - - pubsub::Publisher marker_pub(node, "/marker"); - - pubsub::Publisher pointcloud_pub(node, "/pointcloud"); + // Create the "spinner" which executes callbacks and timers in a background thread + pubsub::BlockingSpinnerWithTimers spinner; + spinner.setNode(node);// Add the node to the spinner - pubsub::Publisher pose_pub(node, "/pose"); + auto start = pubsub::Time::now();// Gets the current time - pubsub::Publisher param_pub(node, "/parameters", true); + // Create a timer which will run at a prescribed interval + spinner.addTimer(1.0/*timer is run every this many seconds*/, [&]() + { + auto now = pubsub::Time::now(); - pubsub::BlockingSpinnerWithTimers spinner; - spinner.setNode(node); + // Build and publish the message + pubsub::msg::String msg; + char value[20]; + sprintf(value, "Hello %f", (now-start).toSec()); + msg.value = value; + string_pub.publish(msg); + }); - // make a parameters message - pubsub::msg::Parameters p; - p.name_length = 3; - p.name = new char*[3]; - p.name[0] = "/test"; - p.name[1] = "/test1"; - p.name[2] = "/test3"; - p.type_length = 3; - p.type = new uint8_t[3]; - p.type[0] = 0; - p.type[1] = 1; - p.type[2] = 2; - p.value_length = 0; - p.min_length = 0; - p.max_length = 0; - param_pub.publish(p); - delete[] p.type; - delete[] p.name; - p.type_length = p.name_length = 0; - p.name = 0; - p.type = 0; + // Wait for the spinner to exit (on control-c) + spinner.run(); - int iteration = 0; - int i = 0; - spinner.addTimer(0.1, [&]() - { - pubsub::msg::String msg; - char value[20]; - sprintf(value, "Hello %i", i++); - msg.value = value; - string_pub.publish(msg); - - ps_sleep(rand()%8); - - //printf("%i clients\n", string_pub.getNumSubscribers()); - - // okay, since we are publishing with shared pointer we actually need to allocate the string properly - /*auto shared = pubsub::msg::StringSharedPtr(new pubsub::msg::String); - shared->value = new char[strlen(msg.value) + 1]; - strcpy(shared->value, msg.value); - string_pub.publish(shared);*/ - - msg.value = 0;// so it doesnt get freed by the destructor since we allocated it ourself - - // generate an image for testing - auto img = pubsub::msg::ImageSharedPtr(new pubsub::msg::Image); - img->width = 100; - img->type = pubsub::msg::Image::R8G8B8; - img->height = 100; - img->data_length = 100*100*3; - img->data = (uint8_t*)malloc(img->data_length); - for (int y = 0; y < 100; y++) - { - for (int x = 0; x < 100; x++) - { - img->data[y*100*3 + x*3] = x*2;// r - img->data[y*100*3 + x*3 + 1] = y*2;// g - img->data[y*100*3 + x*3 + 2] = 0;// b - } - } - image_pub.publish(img); - image_pub2.publish(img); - - auto map = pubsub::msg::CostmapSharedPtr(new pubsub::msg::Costmap); - map->frame = 0; - map->resolution = 0.5; - map->left = 0; - map->bottom = 0; - map->width = 100; - map->height = 100; - map->data_length = map->width*map->height; - map->data = (uint8_t*)malloc(map->data_length); - for (int i = 0; i < map->data_length; i++) - { - map->data[i] = rand()%255; - } - costmap_pub.publish(map); - - auto marker = pubsub::msg::MarkerSharedPtr(new pubsub::msg::Marker); - marker->frame = 1; - marker->id = 0; - marker->marker_type = 0; - marker->data_length = 5*4; - marker->data = (double*)malloc(sizeof(double)*marker->data_length); - for (int i = 0; i < marker->data_length; i += 4) - { - marker->data[i] = i*3; - marker->data[i+1] = i*3; - marker->data[i+2] = (i+4)*3; - marker->data[i+3] = (i+4)*3; - } - marker_pub.publish(marker); - - auto pcld = pubsub::msg::PointCloudSharedPtr(new pubsub::msg::PointCloud); - pcld->point_type = pubsub::msg::PointCloud::POINT_XYZI; - pcld->num_points = 10000; - pcld->data_length = pcld->num_points*4*4; - pcld->data = (uint8_t*)malloc(pcld->data_length); - for (int i = 0; i < pcld->num_points; i++) - { - float* pt = (float*)&pcld->data[i*4*4]; - pt[0] = rand()%100 - 50;//x - pt[1] = rand()%100 - 50;//y - pt[2] = rand()%100 - 50;//z - pt[3] = (rand()%30)/30.0;//i - } - pointcloud_pub.publish(pcld); - pointcloud_pub.publish(pcld); - pointcloud_pub.publish(pcld); - - marker = pubsub::msg::MarkerSharedPtr(new pubsub::msg::Marker); - marker->frame = 1; - marker->id = 1; - marker->marker_type = 1; - marker->data_length = 5*2 + 1; - marker->data = (double*)malloc(sizeof(double)*marker->data_length); - marker->data[0] = 5; - for (int i = 1; i < marker->data_length; i += 2) - { - marker->data[i] = -i*3; - marker->data[i+1] = -i*3; - } - marker_pub.publish(marker); - - auto pose = pubsub::msg::PoseSharedPtr(new pubsub::msg::Pose); - pose->latitude = 0.0; - pose->longitude = 0.0; - pose->z = 10.0; - pose->y = 100.0*sin(iteration*0.01); - pose->x = 100.0*cos(iteration*0.01); - pose->odom_yaw = iteration*0.01; - pose->odom_pitch = 0; - pose->odom_roll = 0; - pose_pub.publish(pose); - - iteration++; - }); - - spinner.wait(); - - return 0; + return 0; } - diff --git a/include/pubsub_cpp/Node.h b/include/pubsub_cpp/Node.h index 2b78652..092ee3d 100644 --- a/include/pubsub_cpp/Node.h +++ b/include/pubsub_cpp/Node.h @@ -400,6 +400,11 @@ class PublisherBase return remapped_topic_; } + ps_pub_t* GetPub() + { + return &publisher_; + } + Node* GetNode() { return node_; diff --git a/src/Publisher.c b/src/Publisher.c index 7fb671b..09f338d 100644 --- a/src/Publisher.c +++ b/src/Publisher.c @@ -140,7 +140,7 @@ void ps_pub_publish_ez(struct ps_pub_t* pub, void* msg) { if (pub->num_clients > 0 || pub->latched) { - struct ps_msg_t data = pub->message_definition->encode(msg, 0); + struct ps_msg_t data = pub->message_definition->encode(msg, pub->allocator); ps_pub_publish(pub, &data); }