From 22caa4c9e1825a945cee9ea0d58d307be21dfee4 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Wed, 3 Sep 2025 13:19:34 +0200 Subject: [PATCH 01/11] Add service client support. Fix generated hashes for service types. --- CMakeLists.txt | 10 +- examples/example_types.h | 54 ++++---- examples/srv_client_add2ints.c | 83 ++++++++++++ .../{srv_add2ints.c => srv_server_add2ints.c} | 4 +- readme.md | 3 +- src/picoparams.c | 18 +-- src/picoros.c | 125 ++++++++++++++++-- src/picoros.h | 71 ++++++++-- tools/type-gen/generate_type_header.py | 12 ++ version.txt | 2 +- 10 files changed, 316 insertions(+), 66 deletions(-) create mode 100644 examples/srv_client_add2ints.c rename examples/{srv_add2ints.c => srv_server_add2ints.c} (97%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 949409d..588d91d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,9 +118,13 @@ if(PICOROS_BUILD_EXAMPLES) thirdparty/Micro-CDR/include ) - add_executable(srv_add2ints examples/srv_add2ints.c ${EXAMPLE_SRC}) - target_include_directories(srv_add2ints PUBLIC ${EXAMPLE_INCLUDE}) - target_link_libraries(srv_add2ints PRIVATE ${EXAMPLE_LIBS}) + add_executable(srv_server_add2ints examples/srv_server_add2ints.c ${EXAMPLE_SRC}) + target_include_directories(srv_server_add2ints PUBLIC ${EXAMPLE_INCLUDE}) + target_link_libraries(srv_server_add2ints PRIVATE ${EXAMPLE_LIBS}) + + add_executable(srv_client_add2ints examples/srv_client_add2ints.c ${EXAMPLE_SRC}) + target_include_directories(srv_client_add2ints PUBLIC ${EXAMPLE_INCLUDE}) + target_link_libraries(srv_client_add2ints PRIVATE ${EXAMPLE_LIBS}) add_executable(talker examples/talker.c ${EXAMPLE_SRC}) target_include_directories(talker PUBLIC ${EXAMPLE_INCLUDE}) diff --git a/examples/example_types.h b/examples/example_types.h index ab1484c..3c4db79 100644 --- a/examples/example_types.h +++ b/examples/example_types.h @@ -1087,7 +1087,7 @@ #define SRV_LIST(SRV, REQUEST, REPLY, FIELD, ARRAY, SEQUENCE) \ SRV(srv_CancelGoal, \ "action_msgs::srv::dds_::CancelGoal", \ - "3d3c84653c1f96918086887e1dcb236faec88b81a5b14fd4cf4840065bcdf8af", \ + "573d8b0a534451d7bc2ac8c5ffde8ac14b8593b7001175d0cd6516dcbeb8689a", \ REQUEST( \ FIELD(ros_GoalInfo, goal_info) \ ), \ @@ -1098,7 +1098,7 @@ ) \ SRV(srv_AddTwoInts, \ "example_interfaces::srv::dds_::AddTwoInts", \ - "000c5fd92d6b2e1a05949348f584d6d652adea1e92d691792011ac2273508302", \ + "e118de6bf5eeb66a2491b5bda11202e7b68f198d6f67922cf30364858239c81a", \ REQUEST( \ FIELD(int64_t, a) \ FIELD(int64_t, b) \ @@ -1109,7 +1109,7 @@ ) \ SRV(srv_SetBool, \ "std_srvs::srv::dds_::SetBool", \ - "c62fbb99d94e1b25e8ef9e109f9581956bb1b3361a45a4e5810c36a90d29932e", \ + "abe9e4bb6b41b40e6789712c00ec8871923e089af3f667a79992a428cff2da0a", \ REQUEST( \ FIELD(bool, data) \ ), \ @@ -1120,7 +1120,7 @@ ) \ SRV(srv_Trigger, \ "std_srvs::srv::dds_::Trigger", \ - "d010825374ce8918e72bfd826c82603e60f45419e932ea976f807b74a863a199", \ + "eeff2cd6fa5ad9d27cdf4dec64818317839b62f212a91e6b5304b634b2062c5f", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1131,7 +1131,7 @@ ) \ SRV(srv_ChangeState, \ "lifecycle_msgs::srv::dds_::ChangeState", \ - "d74c6f524e14cade7b67e7297190360ed749264af300524fc1456f27cc2a553a", \ + "356fe34f0475a43acf54542013af4167b0e729f77ea22ffb045c6ad8e20668e5", \ REQUEST( \ FIELD(ros_Transition, transition) \ ), \ @@ -1141,7 +1141,7 @@ ) \ SRV(srv_GetAvailableStates, \ "lifecycle_msgs::srv::dds_::GetAvailableStates", \ - "c2c0c12de6023822f284929577d4ca2862e0501150010966e8a90b632e4424de", \ + "00a07d79d2207d71e81a8cbc1880e5d924cc16d4688ea8e8e06e443dc8f8aa1d", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1151,7 +1151,7 @@ ) \ SRV(srv_GetAvailableTransitions, \ "lifecycle_msgs::srv::dds_::GetAvailableTransitions", \ - "52a73c0b5d7489bee0df6ddb0aec45c41289d223ff225b58fe11d859ac8109a6", \ + "59b7ecefce0982a8a844b9f2c4f14764c1c4543cc55e72924e2aa4adad83e9bc", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1161,7 +1161,7 @@ ) \ SRV(srv_GetState, \ "lifecycle_msgs::srv::dds_::GetState", \ - "77a9c844e7975f15bc2221e8bbebafc4d118afb78c7f7071eef91f83f940f2f6", \ + "800a0a5aae599782b02932de0caf563f6dc4e7e94b794eadde075ba2cbef9795", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1171,7 +1171,7 @@ ) \ SRV(srv_GetLoggerLevels, \ "rcl_interfaces::srv::dds_::GetLoggerLevels", \ - "490ace424a5f5b14cb4e1e95030d7fe366d8df0fd9fe4d03b44d6009504fd2cc", \ + "03bf1bebd0d6514c7ed0ba7c5e08dc9f2f39c759fe99e1e30ea4157d7674f72d", \ REQUEST( \ SEQUENCE(rstring, names) \ ), \ @@ -1181,7 +1181,7 @@ ) \ SRV(srv_GetParameterTypes, \ "rcl_interfaces::srv::dds_::GetParameterTypes", \ - "87d7ec5ef545d4daa289d500025d31fb2437bc8b8365ca367161c4dac4be33fd", \ + "da199c878688b3e530bdfe3ca8f74cb9fa0c303101e980a9e8f260e25e1c80ca", \ REQUEST( \ SEQUENCE(rstring, names) \ ), \ @@ -1191,7 +1191,7 @@ ) \ SRV(srv_GetParameters, \ "rcl_interfaces::srv::dds_::GetParameters", \ - "135a9e1c3d7cebef792f6687d7ddf99949b21a0bd9a320d863a9110a3f5cf4d6", \ + "bf9803d5c74cf989a5de3e0c2e99444599a627c7ff75f97b8c05b01003675cbc", \ REQUEST( \ SEQUENCE(rstring, names) \ ), \ @@ -1201,7 +1201,7 @@ ) \ SRV(srv_ListParameters, \ "rcl_interfaces::srv::dds_::ListParameters", \ - "a1b0b5d6b967a5b8ac5bc8563c3ea678f349e312a6a4967227c5a96bc5ce38df", \ + "3e6062bfbb27bfb8730d4cef2558221f51a11646d78e7bb30a1e83afac3aad9d", \ REQUEST( \ SEQUENCE(rstring, prefixes) \ FIELD(uint64_t, depth) \ @@ -1212,7 +1212,7 @@ ) \ SRV(srv_SetLoggerLevels, \ "rcl_interfaces::srv::dds_::SetLoggerLevels", \ - "4e7768e0f4c749205f35bc05449f83fad18c42de52c16b1934d66c2f68ffd862", \ + "3ff86cb4e91fbf9abae15c234ecc874448de6ece8e193401c077cf116e4f6d78", \ REQUEST( \ SEQUENCE(ros_LoggerLevel, levels) \ ), \ @@ -1222,7 +1222,7 @@ ) \ SRV(srv_SetParameters, \ "rcl_interfaces::srv::dds_::SetParameters", \ - "130d46386b7e84ea9ecf60608a4d83cbe5e5cb7c6cc5b712b7a4e3a6c254e932", \ + "56eed9a67e169f9cb6c1f987bc88f868c14a8fc9f743a263bc734c154015d7e0", \ REQUEST( \ SEQUENCE(ros_Parameter, parameters) \ ), \ @@ -1232,7 +1232,7 @@ ) \ SRV(srv_SetParametersAtomically, \ "rcl_interfaces::srv::dds_::SetParametersAtomically", \ - "db0bd950ed5ed867a0e90b225e44dea382d762c0762ff3e0ee842190042a59e5", \ + "0e192ef259c07fc3c07a13191d27002222e65e00ccec653ca05e856f79285fcd", \ REQUEST( \ SEQUENCE(ros_Parameter, parameters) \ ), \ @@ -1242,7 +1242,7 @@ ) \ SRV(srv_ListNodes, \ "composition_interfaces::srv::dds_::ListNodes", \ - "57c879a1825b3fa15fd58e70d2e73223af6b9c45c7ae104cc107e73464d3a724", \ + "13f30d73a74c844ee4bacf78f3680f08777cad137f10fbd6a648544cc7b59d2a", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1253,7 +1253,7 @@ ) \ SRV(srv_LoadNode, \ "composition_interfaces::srv::dds_::LoadNode", \ - "7d3496175f54f92d652547e144c9b2a3b22afd604f84ed70adfcd1cfe35801b5", \ + "2e521ef6066fba6d7fc3a3b175d486709f01b4d738d9a59188858546fc8e5acd", \ REQUEST( \ FIELD(rstring, package_name) \ FIELD(rstring, plugin_name) \ @@ -1273,7 +1273,7 @@ ) \ SRV(srv_UnloadNode, \ "composition_interfaces::srv::dds_::UnloadNode", \ - "95d5cfc357956740190c7685ac868d9c7c98dcb8c1416aae8eaa351422074e12", \ + "dce562f83adc735ddcfe9dfb927b89293d4dd0fe2b8fc91e772fb83e970a43b0", \ REQUEST( \ FIELD(uint64_t, unique_id) \ ), \ @@ -1284,7 +1284,7 @@ ) \ SRV(srv_AddDiagnostics, \ "diagnostic_msgs::srv::dds_::AddDiagnostics", \ - "e0b6572a07c2b3ca2c4f2acf742297661f6ceb50f5baa4f9305b6094cf950146", \ + "6b91084c9f8bffd9b0c1f6883e0e8e491a831f4fc6c6aec77a4efaf3f1ea90b5", \ REQUEST( \ FIELD(rstring, load_namespace) \ ), \ @@ -1295,7 +1295,7 @@ ) \ SRV(srv_SelfTest, \ "diagnostic_msgs::srv::dds_::SelfTest", \ - "3bb94b497e74e9beed2dddc7212d3ae596696d30413bd1550fe932fc20efb8a4", \ + "df1e4e54a814a16ccc77471e9239618d46818e8d0e3b45ad6f8c34c6e2abfef6", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1307,7 +1307,7 @@ ) \ SRV(srv_GetMap, \ "nav_msgs::srv::dds_::GetMap", \ - "b531e302f1bd6a81f8a5e03453266c378b1f711b4296000c1c0f53a40e00a7c4", \ + "c8ae77c9995b3554b5ba80e4d4d443f970ac65143102a1d893ec24fc07b31147", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1317,7 +1317,7 @@ ) \ SRV(srv_GetPlan, \ "nav_msgs::srv::dds_::GetPlan", \ - "f1bb9fab287948b4432a4a7e4f529e7ea6269cd06269ee8fcb911fdb4acd23c3", \ + "234f7aff100f5edb8150366601687b027bcdc253db47decb88fff846193fe5e8", \ REQUEST( \ FIELD(ros_PoseStamped, start) \ FIELD(ros_PoseStamped, goal) \ @@ -1329,7 +1329,7 @@ ) \ SRV(srv_LoadMap, \ "nav_msgs::srv::dds_::LoadMap", \ - "0f8fe3aac433c977d7c2f8b57ab1c3a17677e24880d76919b69e4208e99f5132", \ + "1a192ac56c40fed2767dac26f0b371785372276bd465c902676d2dca135aae5a", \ REQUEST( \ FIELD(rstring, map_url) \ ), \ @@ -1340,7 +1340,7 @@ ) \ SRV(srv_SetMap, \ "nav_msgs::srv::dds_::SetMap", \ - "5a83a0c20af4d79248fc3287524bf358b603fb5a05818b5ec85b50fa6e5d0266", \ + "5e11a5b2ca53d8ae85b666a019f16c9904ebc787828f1f566c4e048a1ddedfb4", \ REQUEST( \ FIELD(ros_OccupancyGrid, map) \ FIELD(ros_PoseWithCovarianceStamped, initial_pose) \ @@ -1351,7 +1351,7 @@ ) \ SRV(srv_SetCameraInfo, \ "sensor_msgs::srv::dds_::SetCameraInfo", \ - "27dc5497730fdb6930e66cb879c210e31a83022acc5bbc999f24f55286029f87", \ + "a10cca5d33dc637c8d49db50ab288701a3592bb9cd854f2f16a0659613b68984", \ REQUEST( \ FIELD(ros_CameraInfo, camera_info) \ ), \ @@ -1362,7 +1362,7 @@ ) \ SRV(srv_Empty, \ "std_srvs::srv::dds_::Empty", \ - "458eaf200a3f63d9cc53d19436edebc0c140ef4c93ca75d5bd4ce19fd7759e35", \ + "5888399dedec5ccc85ea6451949fd2c9f97bfdf963f9a588821639fcd31b5d19", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ @@ -1372,7 +1372,7 @@ ) \ SRV(srv_GetInteractiveMarkers, \ "visualization_msgs::srv::dds_::GetInteractiveMarkers", \ - "7fa7e0830e3cbe9c15927a29fdc09fedcd9a86757e375f29ce6b0744cb0b6905", \ + "bfdfaa861f4a8422b4ec3bd02c6681e4a45a83bdc2913d8facdf923d8ad1376a", \ REQUEST( \ FIELD(uint8_t, empty) \ ), \ diff --git a/examples/srv_client_add2ints.c b/examples/srv_client_add2ints.c new file mode 100644 index 0000000..3500568 --- /dev/null +++ b/examples/srv_client_add2ints.c @@ -0,0 +1,83 @@ +/******************************************************************************* + * @file srv_client_add2ints.c + * @brief Example service client for picoros + * @date 2025-May-27 + * + * @details This example demonstrates a ROS service server implementation that + * provides an "add two integers" service functionality. + * + * @copyright Copyright (c) 2025 Ubiquity Robotics + *******************************************************************************/ + +#include +#include +#include "picoros.h" +#include "picoserdes.h" +#include "zenoh-pico/system/common/platform.h" + +// Use command line arguments to change default values +#define MODE "client" +#define LOCATOR "tcp/192.168.1.16:7447" + +// Common utils +extern int picoros_parse_args(int argc, char **argv, picoros_interface_t* ifx); + +// Service reply callback +void add2_client_cb(uint8_t* reply_data, size_t reply_size, bool error); + +// Example service +picoros_srv_client_t add2_client = { + .node_name = "add_two_ints", + .topic = { + .type = ROSTYPE_NAME(srv_AddTwoInts), + .rihs_hash = ROSTYPE_HASH(srv_AddTwoInts), + }, + .user_callback = add2_client_cb, + // .opts = &(z_get_options_t){ + // .timeout_ms = 2000, + // .consolidation.mode = Z_CONSOLIDATION_MODE_MONOTONIC, + // .congestion_control = Z_CONGESTION_CONTROL_BLOCK, + // }, +}; + +void add2_client_cb(uint8_t* reply_data, size_t reply_size, bool error){ + if (error){ + printf("Service error reply recieved\n"); + return; + } + reply_srv_AddTwoInts response = {}; + ps_deserialize(reply_data, &response, reply_size); + printf("Got reply - sum: %ld\n", response.sum); +} + + +int main(int argc, char **argv){ + picoros_interface_t ifx = { + .mode = MODE, + .locator = LOCATOR, + }; + int ret = picoros_parse_args(argc, argv , &ifx); + if(ret != 0){ + return ret; + } + + printf("Starting pico-ros interface %s %s\n", ifx.mode, ifx.locator ); + while (picoros_interface_init(&ifx) == PICOROS_NOT_READY){ + printf("Waiting RMW init...\n"); + z_sleep_s(1); + } + + int a = 0; + int b = 100; + while(true){ + uint8_t buf[100]; + request_srv_AddTwoInts request = {.a=a, b=b}; + size_t len = ps_serialize(buf, &request, 100); + if (picoros_service_call(&add2_client, buf, len) == PICOROS_OK){ + printf("Sent service call...\n"); + a++; + } + z_sleep_s(1); + } + return 0; +} diff --git a/examples/srv_add2ints.c b/examples/srv_server_add2ints.c similarity index 97% rename from examples/srv_add2ints.c rename to examples/srv_server_add2ints.c index 72f1056..6f5bd85 100644 --- a/examples/srv_add2ints.c +++ b/examples/srv_server_add2ints.c @@ -1,5 +1,5 @@ /******************************************************************************* - * @file srv_add2ints.c + * @file srv_server_add2ints.c * @brief Example service server node for picoros * @date 2025-May-27 * @@ -28,7 +28,7 @@ picoros_service_reply_t add2_srv_cb(uint8_t* request, size_t size, void* user_da uint8_t srv_buf[1024]; // Example service -picoros_service_t add2_srv = { +picoros_srv_server_t add2_srv = { .topic = { .name = "add2", .type = ROSTYPE_NAME(srv_AddTwoInts), diff --git a/readme.md b/readme.md index 43e84f8..de68b2d 100644 --- a/readme.md +++ b/readme.md @@ -56,7 +56,8 @@ The project includes several example applications demonstrating different featur - `talker.c` & `talker.cpp`: Basic publisher example - `listener.c`: Basic subscriber example - - `srv_add2ints.c`: Service server example + - `srv_server_add2ints.c`: Service server example + - `srv_client_add2ints.c`: Service client example - `params_server.c`: Parameter server implementation - `odometry_publisher.c` & `odometry_listener.c`: ROS odometry message handling - `batteryState_publisher.c` BatteryState message with sequence fields. diff --git a/src/picoparams.c b/src/picoparams.c index dd93c67..0a4f17b 100644 --- a/src/picoparams.c +++ b/src/picoparams.c @@ -14,14 +14,14 @@ /* Private typedef -----------------------------------------------------------*/ typedef struct { - picoparams_interface_t interface; /**< Parameter interface */ - picoros_service_t get_srv; /**< Get parameter service */ - picoros_service_t list_srv; /**< List parameters service */ - picoros_service_t set_srv; /**< Set parameter service */ - picoros_service_t describe_srv; /**< Describe parameter service */ - picoros_service_t get_types_srv; /**< Get parameter types service */ - picoros_service_t set_atomic_srv; /**< Set atomic parameter service */ - ucdr_writer_t* current_writer; /**< Current writer context */ + picoparams_interface_t interface; /**< Parameter interface */ + picoros_srv_server_t get_srv; /**< Get parameter service */ + picoros_srv_server_t list_srv; /**< List parameters service */ + picoros_srv_server_t set_srv; /**< Set parameter service */ + picoros_srv_server_t describe_srv; /**< Describe parameter service */ + picoros_srv_server_t get_types_srv; /**< Get parameter types service */ + picoros_srv_server_t set_atomic_srv; /**< Set atomic parameter service */ + ucdr_writer_t* current_writer; /**< Current writer context */ } picoparams_server_t; /* Private define ------------------------------------------------------------*/ @@ -391,7 +391,7 @@ static size_t describe_params(picoparams_server_t* server, ucdrBuffer* reader, u picoros_service_reply_t params_server_handler(uint8_t* request_data, size_t request_size, void* user_data) { picoros_service_reply_t reply = {}; - picoros_service_t* s = (picoros_service_t*)user_data; + picoros_srv_server_t* s = (picoros_srv_server_t*)user_data; ucdrBuffer querry_writter = {}; ucdrBuffer querry_reader = {}; diff --git a/src/picoros.c b/src/picoros.c index 10d1421..b98d2ea 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -11,15 +11,13 @@ *******************************************************************************/ /* Private includes ----------------------------------------------------------*/ +#include #include -#include #include #include "picoros.h" -#include "zenoh-pico/config.h" -#include "zenoh-pico/system/common/platform.h" -#include "zenoh-pico/utils/result.h" #ifdef PICOROS_DEBUG + #include #define _PR_LOG(...) printf(__VA_ARGS__) #else #define _PR_LOG(...) @@ -72,8 +70,14 @@ static int rmw_zenoh_topic_keyexpr(picoros_node_t* node, rmw_topic_t* topic, cha } static int rmw_zenoh_service_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char* keyexpr) { - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, - topic->type, topic->rihs_hash); + if (topic->name == NULL){ + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, node->name, + topic->type, topic->rihs_hash); + } + else{ + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, + topic->type, topic->rihs_hash); + } } static int rmw_zenoh_topic_liveliness_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char *keyexpr, const char *entity_str) { @@ -145,8 +149,8 @@ static void sub_data_handler(z_loaned_sample_t *sample, void *ctx) { z_free(raw_data); } -static void srv_data_handler(z_loaned_query_t *query, void *arg) { - picoros_service_t* srv = (picoros_service_t*)arg; +static void queriable_data_handler(z_loaned_query_t *query, void *arg) { + picoros_srv_server_t* srv = (picoros_srv_server_t*)arg; if (srv->user_callback == NULL){ return; @@ -191,7 +195,47 @@ static void srv_data_handler(z_loaned_query_t *query, void *arg) { z_free(rx_data); } -static void srv_drop_handler(void* arg) { _PR_LOG("Drop srv callback\n"); } +static void queriable_drop_handler(void* arg) { _PR_LOG("Drop srv callback\n"); } + +static void get_drop_handler(void* ctx){ + _PR_LOG("Drop get callback\n"); + picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; + client->in_progress = false; +} + +static void get_data_handler(z_loaned_reply_t *reply, void *ctx){ + if (ctx == NULL){ + return; + } + _PR_LOG("Get data handler\n"); + size_t raw_data_len = 0; + uint8_t* raw_data = 0; + bool error = false; + const z_loaned_sample_t* sample = 0; + const z_loaned_bytes_t* payload = 0; + const z_loaned_reply_err_t* err = 0; + + if (z_reply_is_ok(reply)) { + sample = z_reply_ok(reply); + payload = z_sample_payload(sample); + } + else { + err = z_reply_err(reply); + payload = z_reply_err_payload(err); + error = true; + } + + raw_data_len = _z_bytes_len(payload); + if (raw_data_len == 0) { + return; + } + raw_data = (uint8_t*)z_malloc(raw_data_len); + _z_bytes_to_buf(payload, raw_data, raw_data_len); + + picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; + client->user_callback(raw_data, raw_data_len, error); + z_free(raw_data); +} /* Public functions ----------------------------------------------------------*/ @@ -344,7 +388,7 @@ picoros_res_t picoros_subscriber_declare(picoros_node_t* node, picoros_subscribe return PICOROS_OK; } -picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_service_t* srv) { +picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t* srv) { z_result_t res; char keyexpr[KEYEXPR_SIZE]; @@ -363,7 +407,7 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_service_t* s options.complete = true; // needed for rmw_zenoh z_owned_closure_query_t callback; - z_closure_query(&callback, srv_data_handler, srv_drop_handler, srv); + z_closure_query(&callback, queriable_data_handler, queriable_drop_handler, srv); if ((res = z_declare_queryable(z_session_loan(&s_wrapper), &srv->zqable, z_view_keyexpr_loan(&ke), z_closure_query_move(&callback), &options)) != Z_OK) { _PR_LOG("Unable to declare service! Error:%d\n", res); @@ -382,6 +426,65 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_service_t* s return PICOROS_OK; } +picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* payload, size_t len){ + if (client == NULL) { return PICOROS_ERROR;} + if (client->in_progress) { return PICOROS_NOT_READY;} + + z_result_t res; + char keyexpr[KEYEXPR_SIZE]; + + // Generate key expressions + z_view_keyexpr_t ke; + if (client->topic.type != NULL) { + picoros_node_t node = { + .domain_id = client->node_domain_id, + .name = client->node_name, + }; + rmw_zenoh_service_keyexpr(&node, &client->topic, keyexpr); + z_view_keyexpr_from_str_unchecked(&ke, keyexpr); + } + else { + z_view_keyexpr_from_str_unchecked(&ke, client->topic.name); + } + + // Default options + z_get_options_t default_opts; + z_get_options_default(&default_opts); + z_get_options_t* opts = &default_opts; + if (client->opts != NULL){ + opts = client->opts; + } + + // Payload + z_owned_bytes_t zbytes; + z_bytes_copy_from_buf(&zbytes, payload, len); + opts->payload = z_bytes_move(&zbytes); + + // RMW attachment + rmw_attachment_t attachment = { + .rmw_gid_size = 16, + .sequence_number = 1, + .time = z_clock_now().tv_nsec, + }; + z_owned_bytes_t tx_attachment; + z_bytes_copy_from_buf(&tx_attachment, (uint8_t*)&attachment, sizeof(rmw_attachment_t)); + opts->attachment = z_bytes_move(&tx_attachment); + + // Closure + z_owned_closure_reply_t callback; + z_closure(&callback, get_data_handler, get_drop_handler, client); + + client->in_progress = true; + if ((res = z_get(z_session_loan(&s_wrapper), z_view_keyexpr_loan(&ke), "", z_closure_reply_move(&callback), opts)) != Z_OK) { + _PR_LOG("Error calling %s service! Error:%d\n", client->topic.name, res); + client->in_progress = false; + z_drop(opts->attachment); + z_drop(opts->payload); + return PICOROS_ERROR; + } + return PICOROS_OK; +} + picoros_res_t picoros_unsubscribe(picoros_subscriber_t* sub) { return (z_undeclare_subscriber(z_subscriber_move(&sub->zsub)) == Z_OK) ? PICOROS_OK : PICOROS_ERROR; } diff --git a/src/picoros.h b/src/picoros.h index ca0036a..7788e0d 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -28,7 +28,9 @@ /** @} */ /* Exported includes ---------------------------------------------------------*/ + #include +#include /* Exported constants --------------------------------------------------------*/ /** @brief Maximum size for key expressions used in topic names @ingroup picoros */ @@ -89,25 +91,60 @@ typedef struct { * @param user_data User data passed during service registration * @return Service reply structure containing response data */ -typedef picoros_service_reply_t (*picoros_service_cb_t)( - uint8_t* request_data, - size_t reqest_size, - void* user_data - ); +typedef picoros_service_reply_t (*picoros_srv_server_cb_t)( + uint8_t* request_data, + size_t reqest_size, + void* user_data +); /** * @brief Service server structure for Pico-ROS */ typedef struct { - z_owned_queryable_t zqable; /**< Zenoh queryable instance */ - rmw_topic_t topic; /**< Topic information */ - rmw_attachment_t attachment; /**< RMW attachment data */ - void* user_data; /**< User data for callback */ - picoros_service_cb_t user_callback; /**< User callback for service handling */ -} picoros_service_t; + z_owned_queryable_t zqable; /**< Zenoh queryable instance */ + rmw_topic_t topic; /**< Topic information */ + rmw_attachment_t attachment; /**< RMW attachment data */ + void* user_data; /**< User data for callback */ + picoros_srv_server_cb_t user_callback; /**< User callback for service handling */ +} picoros_srv_server_t; + +/** @} */ + /** @} */ +/** + * @defgroup service_client Service client + * @ingroup picoros + * @{ + */ + +/** + * @brief Callback function type for service reply handling + * @param reply_data Pointer to received reply data (CDR encoded) + * @param reply_size Size of received reply + * @param error Received error reply + * @return void + */ +typedef void (*picoros_srv_client_cb_t)( + uint8_t* reply_data, + size_t reply_size, + bool error +); + +/** + * @brief Service client structure for Pico-ROS + */ +typedef struct { + char* node_name; /**< Node name of service server */ + uint32_t node_domain_id; /**< Domain ID of service server */ + rmw_topic_t topic; /**< Topic information */ + picoros_srv_client_cb_t user_callback; /**< User callback for service reply handling */ + bool in_progress; /**< Limit to one ongoing request */ + z_get_options_t* opts; /**< Request options */ +} picoros_srv_client_t; + + /** @} */ /** * @brief Publisher structure for Pico-ROS @ingroup picoros @@ -258,7 +295,17 @@ picoros_res_t picoros_unsubscribe(picoros_subscriber_t *sub); * @return PICOROS_OK on success, error code otherwise * @ingroup service_server */ -picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_service_t* srv); + picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t* srv); + + /** + * @brief Call service using service client. + * @param node Pointer to node instance + * @param srv Pointer to service configuration + * @return PICOROS_OK on success, error code otherwise + * @ingroup service_server + * @details client + */ +picoros_res_t picoros_service_call(picoros_srv_client_t* client, uint8_t* payload, size_t len); #ifdef __cplusplus } diff --git a/tools/type-gen/generate_type_header.py b/tools/type-gen/generate_type_header.py index d496e2e..1fa6d40 100755 --- a/tools/type-gen/generate_type_header.py +++ b/tools/type-gen/generate_type_header.py @@ -517,6 +517,7 @@ def parse_type_descriptions(output_dir: str) -> Dict[str, Dict]: # 166: 'bounded_wstring_unbounded_sequence', # FIELD_TYPE_BOUNDED_WSTRING_UNBOUNDED_SEQUENCE } + srv_hashes = {} for json_file in output_path.rglob('*.json'): try: with open(json_file, 'r') as f: @@ -525,9 +526,15 @@ def parse_type_descriptions(output_dir: str) -> Dict[str, Dict]: if 'type_hashes' in data and 'type_description_msg' in data: type_hashes = {th['type_name']: th['hash_string'].replace('RIHS01_', '') for th in data['type_hashes']} + # Process main type description main_desc = data['type_description_msg']['type_description'] type_name = main_desc['type_name'] + + # Save main srv hash + if "/srv/" in type_name: + srv_hashes[type_name] = type_hashes[type_name] + skip = False if 'referenced_type_descriptions' in data['type_description_msg']: ref_types = data['type_description_msg']['referenced_type_descriptions'] @@ -549,6 +556,11 @@ def parse_type_descriptions(output_dir: str) -> Dict[str, Dict]: 'fields': fields, 'original_name': type_name } + if "_Request" in type_name: + type_info[type_name]['hash'] = srv_hashes[type_name[:-8]] + elif "_Response" in type_name: + type_info[type_name]['hash'] = srv_hashes[type_name[:-9]] + else: print(f"Skipping {type_name}, incompatible field types") diff --git a/version.txt b/version.txt index 341cf11..9325c3c 100644 --- a/version.txt +++ b/version.txt @@ -1 +1 @@ -0.2.0 \ No newline at end of file +0.3.0 \ No newline at end of file From 09e039b9d4e9748e15d1d80c45ef92bac96c8e9c Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Wed, 3 Sep 2025 21:11:35 +0200 Subject: [PATCH 02/11] picoros_service_call_in_progress function. Small cleanup. --- docs/Doxyfile | 2 +- src/picoros.c | 16 +++++++++------- src/picoros.h | 43 ++++++++++++++++++++++++------------------- 3 files changed, 34 insertions(+), 27 deletions(-) diff --git a/docs/Doxyfile b/docs/Doxyfile index b00a01b..f9f1bd6 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -1,6 +1,6 @@ # Project settings PROJECT_NAME = "Pico-ROS" -PROJECT_NUMBER = $(shell cat ../version.txt) +PROJECT_NUMBER = "" PROJECT_BRIEF = "A lightweight ROS client implementation for resource-constrained devices" OUTPUT_DIRECTORY = generated CREATE_SUBDIRS = YES diff --git a/src/picoros.c b/src/picoros.c index b98d2ea..fb605db 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -198,16 +198,14 @@ static void queriable_data_handler(z_loaned_query_t *query, void *arg) { static void queriable_drop_handler(void* arg) { _PR_LOG("Drop srv callback\n"); } static void get_drop_handler(void* ctx){ - _PR_LOG("Drop get callback\n"); picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; - client->in_progress = false; + client->_in_progress = false; } static void get_data_handler(z_loaned_reply_t *reply, void *ctx){ if (ctx == NULL){ return; } - _PR_LOG("Get data handler\n"); size_t raw_data_len = 0; uint8_t* raw_data = 0; bool error = false; @@ -428,7 +426,7 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* payload, size_t len){ if (client == NULL) { return PICOROS_ERROR;} - if (client->in_progress) { return PICOROS_NOT_READY;} + if (client->_in_progress) { return PICOROS_NOT_READY;} z_result_t res; char keyexpr[KEYEXPR_SIZE]; @@ -462,7 +460,7 @@ picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* paylo // RMW attachment rmw_attachment_t attachment = { - .rmw_gid_size = 16, + .rmw_gid_size = RMW_GID_SIZE, .sequence_number = 1, .time = z_clock_now().tv_nsec, }; @@ -474,10 +472,10 @@ picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* paylo z_owned_closure_reply_t callback; z_closure(&callback, get_data_handler, get_drop_handler, client); - client->in_progress = true; + client->_in_progress = true; if ((res = z_get(z_session_loan(&s_wrapper), z_view_keyexpr_loan(&ke), "", z_closure_reply_move(&callback), opts)) != Z_OK) { _PR_LOG("Error calling %s service! Error:%d\n", client->topic.name, res); - client->in_progress = false; + client->_in_progress = false; z_drop(opts->attachment); z_drop(opts->payload); return PICOROS_ERROR; @@ -485,6 +483,10 @@ picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* paylo return PICOROS_OK; } +bool picoros_service_call_in_progress(picoros_srv_client_t* client){ + return client->_in_progress; +} + picoros_res_t picoros_unsubscribe(picoros_subscriber_t* sub) { return (z_undeclare_subscriber(z_subscriber_move(&sub->zsub)) == Z_OK) ? PICOROS_OK : PICOROS_ERROR; } diff --git a/src/picoros.h b/src/picoros.h index 7788e0d..b06e946 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -42,7 +42,7 @@ /* Exported types ------------------------------------------------------------*/ - /** +/** * @defgroup rmw RMW support * @ingroup picoros * @{ @@ -69,7 +69,7 @@ typedef struct { /** @} */ - /** +/** * @defgroup service_server Service server * @ingroup picoros * @{ @@ -111,8 +111,6 @@ typedef struct { /** @} */ -/** @} */ - /** * @defgroup service_client Service client * @ingroup picoros @@ -140,11 +138,11 @@ typedef struct { uint32_t node_domain_id; /**< Domain ID of service server */ rmw_topic_t topic; /**< Topic information */ picoros_srv_client_cb_t user_callback; /**< User callback for service reply handling */ - bool in_progress; /**< Limit to one ongoing request */ - z_get_options_t* opts; /**< Request options */ + bool _in_progress; /**< Private flag used to limit to one ongoing request */ + z_get_options_t* opts; /**< Request options, if NULL default options are used */ } picoros_srv_client_t; - /** @} */ +/** @} */ /** * @brief Publisher structure for Pico-ROS @ingroup picoros @@ -163,6 +161,7 @@ typedef struct { * @ingroup picoros * @{ */ + /** * @brief Callback function type for subscriber data handling * @param rx_data Pointer to received data buffer (CDR encoded) @@ -201,12 +200,12 @@ typedef struct { /** @} */ - /** * @defgroup interface Network interface * @ingroup picoros * @{ */ + /** * @brief Network interface configuration */ @@ -217,8 +216,6 @@ typedef struct { /** @} */ - - /** * @brief Result codes for Pico-ROS operations @ingroup picoros */ @@ -295,17 +292,25 @@ picoros_res_t picoros_unsubscribe(picoros_subscriber_t *sub); * @return PICOROS_OK on success, error code otherwise * @ingroup service_server */ - picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t* srv); +picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t* srv); - /** - * @brief Call service using service client. - * @param node Pointer to node instance - * @param srv Pointer to service configuration - * @return PICOROS_OK on success, error code otherwise - * @ingroup service_server - * @details client - */ +/** + * @brief Call service using service client. + * @param client Pointer to client instance. Should be in scope until service call is ongoing. + * @param payload Pointer to data payload + * @param len Size of data + * @return PICOROS_OK on success, PICOROS_NOT_READY when request already progress, error code otherwise + * @ingroup service_client + */ picoros_res_t picoros_service_call(picoros_srv_client_t* client, uint8_t* payload, size_t len); + +/** + * @brief Check if client has ongoing service call. + * @param client Pointer to client instance. + * @return true if request is in progress + * @ingroup service_client + */ +bool picoros_service_call_in_progress(picoros_srv_client_t* client); #ifdef __cplusplus } From 7f22bd70b9e25a30f606d69096a5fe0db19beb87 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Wed, 3 Sep 2025 22:23:45 +0200 Subject: [PATCH 03/11] Fix example header. --- examples/srv_client_add2ints.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/srv_client_add2ints.c b/examples/srv_client_add2ints.c index 3500568..82ad197 100644 --- a/examples/srv_client_add2ints.c +++ b/examples/srv_client_add2ints.c @@ -1,10 +1,10 @@ /******************************************************************************* * @file srv_client_add2ints.c * @brief Example service client for picoros - * @date 2025-May-27 + * @date 2025-Sept-2 * - * @details This example demonstrates a ROS service server implementation that - * provides an "add two integers" service functionality. + * @details This example demonstrates a ROS service client implementation that + * calls an "add two integers" service and prints the result. * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ From d50893b4c1ee9109239c0996558d7b77a6113947 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Thu, 4 Sep 2025 18:23:26 +0200 Subject: [PATCH 04/11] Service callback refactoring... - Refactor service server and client callbacks to include their instances instad of only user_data. This frees up user_data in case we want to have single callback handling multiple services. - Update picoparams and examples with above changes. - Include service call drop callback that the user can provide. - Clean up some comments. --- examples/srv_client_add2ints.c | 14 +++--- examples/srv_server_add2ints.c | 10 ++--- src/picoparams.c | 27 +++++------ src/picoros.c | 11 +++-- src/picoros.h | 82 +++++++++++++++++++--------------- 5 files changed, 75 insertions(+), 69 deletions(-) diff --git a/examples/srv_client_add2ints.c b/examples/srv_client_add2ints.c index 82ad197..1f1863b 100644 --- a/examples/srv_client_add2ints.c +++ b/examples/srv_client_add2ints.c @@ -2,13 +2,13 @@ * @file srv_client_add2ints.c * @brief Example service client for picoros * @date 2025-Sept-2 - * + * * @details This example demonstrates a ROS service client implementation that * calls an "add two integers" service and prints the result. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ - + #include #include #include "picoros.h" @@ -23,7 +23,7 @@ extern int picoros_parse_args(int argc, char **argv, picoros_interface_t* ifx); // Service reply callback -void add2_client_cb(uint8_t* reply_data, size_t reply_size, bool error); +void add2_client_cb(picoros_srv_client_t* client, uint8_t* reply_data, size_t reply_size, bool error); // Example service picoros_srv_client_t add2_client = { @@ -40,10 +40,10 @@ picoros_srv_client_t add2_client = { // }, }; -void add2_client_cb(uint8_t* reply_data, size_t reply_size, bool error){ +void add2_client_cb(picoros_srv_client_t* client, uint8_t* reply_data, size_t reply_size, bool error){ if (error){ printf("Service error reply recieved\n"); - return; + return; } reply_srv_AddTwoInts response = {}; ps_deserialize(reply_data, &response, reply_size); @@ -66,7 +66,7 @@ int main(int argc, char **argv){ printf("Waiting RMW init...\n"); z_sleep_s(1); } - + int a = 0; int b = 100; while(true){ diff --git a/examples/srv_server_add2ints.c b/examples/srv_server_add2ints.c index 6f5bd85..e367b52 100644 --- a/examples/srv_server_add2ints.c +++ b/examples/srv_server_add2ints.c @@ -2,13 +2,13 @@ * @file srv_server_add2ints.c * @brief Example service server node for picoros * @date 2025-May-27 - * + * * @details This example demonstrates a ROS service server implementation that * provides an "add two integers" service functionality. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ - + #include #include #include "picoros.h" @@ -22,7 +22,7 @@ extern int picoros_parse_args(int argc, char **argv, picoros_interface_t* ifx); // Service callback -picoros_service_reply_t add2_srv_cb(uint8_t* request, size_t size, void* user_data); +picoros_service_reply_t add2_srv_cb(picoros_srv_server_t* server, uint8_t* request, size_t size); // Static buffer for service reply serialization, used from zenoh threads uint8_t srv_buf[1024]; @@ -43,7 +43,7 @@ picoros_node_t node = { }; // Service callback -picoros_service_reply_t add2_srv_cb(uint8_t* rx_data, size_t rx_size, void* user_data){ +picoros_service_reply_t add2_srv_cb(picoros_srv_server_t* server, uint8_t* rx_data, size_t rx_size){ // request, response structs request_srv_AddTwoInts request = {}; reply_srv_AddTwoInts response = {}; diff --git a/src/picoparams.c b/src/picoparams.c index 0a4f17b..b1cf340 100644 --- a/src/picoparams.c +++ b/src/picoparams.c @@ -2,10 +2,10 @@ * @file picoparams.c * @brief Pico-ROS Parameter Server Implementation * @date 2025-Jun-01 - * + * * @details This file implements the ROS 2 parameter server functionality, * providing parameter storage, retrieval, and manipulation capabilities. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ @@ -28,7 +28,7 @@ typedef struct { /* Private macro -------------------------------------------------------------*/ /* Private constants ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ -picoros_service_reply_t params_server_handler(uint8_t* request_data, size_t request_size, void* user_data); +picoros_service_reply_t params_server_handler(picoros_srv_server_t* server, uint8_t* request_data, size_t request_size); /* Private variables ---------------------------------------------------------*/ /* Parameter topic names and hashes */ @@ -40,7 +40,6 @@ picoparams_server_t pserver = { .rihs_hash = "bf9803d5c74cf989a5de3e0c2e99444599a627c7ff75f97b8c05b01003675cbc", }, .user_callback = params_server_handler, - .user_data = &pserver.get_srv, }, .list_srv = { .topic = { @@ -49,7 +48,6 @@ picoparams_server_t pserver = { .rihs_hash = "3e6062bfbb27bfb8730d4cef2558221f51a11646d78e7bb30a1e83afac3aad9d", }, .user_callback = params_server_handler, - .user_data = &pserver.list_srv, }, .set_srv = { .topic = { @@ -58,7 +56,6 @@ picoparams_server_t pserver = { .rihs_hash = "56eed9a67e169f9cb6c1f987bc88f868c14a8fc9f743a263bc734c154015d7e0", }, .user_callback = params_server_handler, - .user_data = &pserver.set_srv, }, .describe_srv = { .topic = { @@ -67,7 +64,6 @@ picoparams_server_t pserver = { .rihs_hash = "845b484d71eb0673dae682f2e3ba3c4851a65a3dcfb97bddd82c5b57e91e4cff", }, .user_callback = params_server_handler, - .user_data = &pserver.describe_srv, }, .set_atomic_srv = { .topic = { @@ -76,7 +72,6 @@ picoparams_server_t pserver = { .rihs_hash = "0e192ef259c07fc3c07a13191d27002222e65e00ccec653ca05e856f79285fcd", }, .user_callback = params_server_handler, - .user_data = &pserver.set_atomic_srv, }, .get_types_srv = { .topic = { @@ -85,7 +80,6 @@ picoparams_server_t pserver = { .rihs_hash = "da199c878688b3e530bdfe3ca8f74cb9fa0c303101e980a9e8f260e25e1c80ca", }, .user_callback = params_server_handler, - .user_data = &pserver.get_types_srv, }, }; @@ -389,9 +383,8 @@ static size_t describe_params(picoparams_server_t* server, ucdrBuffer* reader, u -picoros_service_reply_t params_server_handler(uint8_t* request_data, size_t request_size, void* user_data) { +picoros_service_reply_t params_server_handler(picoros_srv_server_t* server, uint8_t* request_data, size_t request_size) { picoros_service_reply_t reply = {}; - picoros_srv_server_t* s = (picoros_srv_server_t*)user_data; ucdrBuffer querry_writter = {}; ucdrBuffer querry_reader = {}; @@ -399,22 +392,22 @@ picoros_service_reply_t params_server_handler(uint8_t* request_data, size_t requ *((uint32_t*)pserver.interface.reply_buf) = 0x0100; ucdr_init_buffer(&querry_writter, pserver.interface.reply_buf + 4, pserver.interface.reply_buf_size - 4); size_t ret = 0; - if (s == &pserver.list_srv){ + if (server == &pserver.list_srv){ ret = list_params(&pserver, &querry_reader, &querry_writter); } - else if(s == &pserver.describe_srv){ + else if(server == &pserver.describe_srv){ ret = describe_params(&pserver, &querry_reader, &querry_writter); } - else if(s == &pserver.get_srv){ + else if(server == &pserver.get_srv){ ret = get_params_value(&pserver, &querry_reader, &querry_writter); } - else if(s == &pserver.set_srv){ + else if(server == &pserver.set_srv){ ret = set_params_value(&pserver, &querry_reader, &querry_writter); } - else if(s == &pserver.get_types_srv){ + else if(server == &pserver.get_types_srv){ ret = get_params_type(&pserver, &querry_reader, &querry_writter); } - else if(s == &pserver.set_atomic_srv){ + else if(server == &pserver.set_atomic_srv){ // Not implemented } diff --git a/src/picoros.c b/src/picoros.c index fb605db..edfaf6e 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -2,11 +2,11 @@ * @file picoros.c * @brief Pico-ROS Core Implementation * @date 2025-May-27 - * + * * @details This file implements the core functionality of Pico-ROS, including * node management, publisher/subscriber communication, and service * server implementation. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ @@ -164,7 +164,7 @@ static void queriable_data_handler(z_loaned_query_t *query, void *arg) { _z_bytes_to_buf(b, rx_data, rx_data_len); // process - picoros_service_reply_t reply = srv->user_callback(rx_data, rx_data_len, srv->user_data); + picoros_service_reply_t reply = srv->user_callback(srv, rx_data, rx_data_len); if (reply.data) { // move reply to zbytes @@ -200,6 +200,9 @@ static void queriable_drop_handler(void* arg) { _PR_LOG("Drop srv callback\n"); static void get_drop_handler(void* ctx){ picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; client->_in_progress = false; + if(client->drop_callback != NULL){ + client->drop_callback(client); + } } static void get_data_handler(z_loaned_reply_t *reply, void *ctx){ @@ -231,7 +234,7 @@ static void get_data_handler(z_loaned_reply_t *reply, void *ctx){ _z_bytes_to_buf(payload, raw_data, raw_data_len); picoros_srv_client_t* client = (picoros_srv_client_t*)ctx; - client->user_callback(raw_data, raw_data_len, error); + client->user_callback(client, raw_data, raw_data_len, error); z_free(raw_data); } diff --git a/src/picoros.h b/src/picoros.h index b06e946..c7ab696 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -2,12 +2,12 @@ * @file picoros.h * @brief Pico-ROS - A lightweight ROS client implementation for resource-constrained devices * @date 2025-May-27 - * + * * @details This header file provides the core functionality for Pico-ROS, including: * - Node management * - Publisher/Subscriber communication * - Service server implementation - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ @@ -62,9 +62,9 @@ typedef struct __attribute__((__packed__)) { * @brief RMW topic structure required by rmw_zenoh */ typedef struct { - const char* name; /**< Topic name */ - const char* type; /**< Message type */ - const char* rihs_hash; /**< RIHS hash */ + const char* name; /**< Topic name */ + const char* type; /**< Message type */ + const char* rihs_hash; /**< RIHS hash */ } rmw_topic_t; /** @} */ @@ -74,6 +74,10 @@ typedef struct { * @ingroup picoros * @{ */ + +/* Forward declaration */ +struct picoros_srv_server_s; + /** * @brief Service reply data structure */ @@ -85,26 +89,23 @@ typedef struct { } picoros_service_reply_t; /** - * @brief Callback function type for service request handling - * @param request_data Pointer to received request data (CDR encoded) - * @param reqest_size Size of received request - * @param user_data User data passed during service registration - * @return Service reply structure containing response data - */ +* @brief Callback function type for service request handling +* @return Service reply structure containing response data +*/ typedef picoros_service_reply_t (*picoros_srv_server_cb_t)( - uint8_t* request_data, - size_t reqest_size, - void* user_data + struct picoros_srv_server_s* server, /**< Pointer to server instance */ + uint8_t* request_data, /**< Pointer to received request data */ + size_t reqest_size /**< Request data size */ ); /** * @brief Service server structure for Pico-ROS */ -typedef struct { +typedef struct picoros_srv_server_s{ z_owned_queryable_t zqable; /**< Zenoh queryable instance */ rmw_topic_t topic; /**< Topic information */ rmw_attachment_t attachment; /**< RMW attachment data */ - void* user_data; /**< User data for callback */ + void* user_data; /**< User data, not used by picoros */ picoros_srv_server_cb_t user_callback; /**< User callback for service handling */ } picoros_srv_server_t; @@ -117,29 +118,40 @@ typedef struct { * @{ */ +/* Forward declaration */ +struct picoros_srv_client_s; + /** * @brief Callback function type for service reply handling - * @param reply_data Pointer to received reply data (CDR encoded) - * @param reply_size Size of received reply - * @param error Received error reply * @return void */ typedef void (*picoros_srv_client_cb_t)( - uint8_t* reply_data, - size_t reply_size, - bool error + struct picoros_srv_client_s* client, /**< Pointer to client starting the request */ + uint8_t* reply_data, /**< Pointer to received reply data (CDR encoded) */ + size_t reply_size, /**< Size of received reply */ + bool error /**< Received error reply */ ); /** - * @brief Service client structure for Pico-ROS + * @brief Callback function type for service reply dropped handling + * @param client picoros_srv_client_t that started the request + * @return void */ -typedef struct { - char* node_name; /**< Node name of service server */ - uint32_t node_domain_id; /**< Domain ID of service server */ - rmw_topic_t topic; /**< Topic information */ - picoros_srv_client_cb_t user_callback; /**< User callback for service reply handling */ - bool _in_progress; /**< Private flag used to limit to one ongoing request */ - z_get_options_t* opts; /**< Request options, if NULL default options are used */ +typedef void (*picoros_srv_client_drop_cb_t)( struct picoros_srv_client_s* client); + + +/** +* @brief Service client structure for Pico-ROS + */ +typedef struct picoros_srv_client_s{ + char* node_name; /**< Node name of service server */ + uint32_t node_domain_id; /**< Domain ID of service server */ + rmw_topic_t topic; /**< Topic information */ + picoros_srv_client_cb_t user_callback; /**< User callback for service reply handling. Called if reply is received. */ + picoros_srv_client_drop_cb_t drop_callback; /**< User callback for service call drop handling. Called for every service call.*/ + bool _in_progress; /**< Private flag used to limit to one ongoing request */ + z_get_options_t* opts; /**< Request options, if NULL default options are used */ + void* user_data; /**< User data, not used by picoros */ } picoros_srv_client_t; /** @} */ @@ -164,12 +176,10 @@ typedef struct { /** * @brief Callback function type for subscriber data handling - * @param rx_data Pointer to received data buffer (CDR encoded) - * @param data_len Size of received data in bytes */ typedef void (*picoros_sub_cb_t)( - uint8_t* rx_data, - size_t data_len + uint8_t* rx_data, /**< Pointer to received data buffer (CDR encoded) */ + size_t data_len /**< Size of received data in bytes */ ); /** @@ -246,7 +256,7 @@ void picoros_interface_shutdown(void); * @param node Pointer to node configuration * @return PICOROS_OK on success, error code otherwise * @ingroup node - */ + */ picoros_res_t picoros_node_init(picoros_node_t* node); /** @@ -303,7 +313,7 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t * @ingroup service_client */ picoros_res_t picoros_service_call(picoros_srv_client_t* client, uint8_t* payload, size_t len); - + /** * @brief Check if client has ongoing service call. * @param client Pointer to client instance. From 987d6ea55a45ae82ec0cb46545101a1ed2d9c773 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Fri, 5 Sep 2025 11:59:14 +0200 Subject: [PATCH 05/11] Update readme --- readme.md | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/readme.md b/readme.md index de68b2d..5af94c5 100644 --- a/readme.md +++ b/readme.md @@ -1,10 +1,12 @@

PicoROS logo -

+

## Overview **Pico-ROS** is a lightweight ROS (Robot Operating System) client implementation designed for resource-constrained devices. Built on top of [zenoh-pico](https://github.com/eclipse-zenoh/zenoh-pico) and working in conjunction with [rmw-zenoh](https://github.com/ros2/rmw_zenoh), it provides an easy-to-use interface to ROS host from embedded systems. +Pico-ROS targets resource-constrained microcontroller-based nodes and edge devices in ROS networks. + ## Features ### Core Components @@ -15,6 +17,7 @@ - Publisher - Subscriber - Service server + - Service client 2. **picoserdes** - CDR serialization/deserialization for ROS messages @@ -112,23 +115,24 @@ The project uses Doxygen for documentation. To generate and read: xdg-open docs/generated/html/index.html ``` -## Use Cases - -Pico-ROS is ideal for resource-constrained microcontroller-based ROS nodes and edge devices in ROS networks. - -While it provides core ROS functionality, it's not intended to replace full ROS implementations like [rclc](https://github.com/ros2/rclc) or [rclcpp](https://github.com/ros2/rclcpp) for systems with more resources. ## Using in your project -See the examples directory for implementation references. +See the [examples](examples/) directory for implementation references. Automatic generation of `user_types.h` file can be done using scripts in [/tools/type-gen](tools/type-gen/readme.md). Format of the file is descibed in [picoserdes.h](src/picoserdes.h). Example with standard ROS types can be found in [example_types.h](examples/example_types.h). -For compatibility with `rmw_zenohd` default configuration, set matching zenoh-pico lease duration settings with defines or edit the zenoh-pico `config.h` file: +For compatibility with `rmw_zenohd` default configuration, set matching zenoh-pico lease duration settings with defines or edit the zenoh-pico `config.h` file: - `-DZ_TRANSPORT_LEASE=60000` and - `-DZ_TRANSPORT_LEASE_EXPIRE_FACTOR=2` -Examples of projects using Pico-ROS can be found in [picoros-examples](https://github.com/Pico-ROS/picoros-examples). +## Example projects + +- [picoros-examples](https://github.com/Pico-ROS/picoros-examples) +- [roscon2025_control_workshop](https://github.com/ros-controls/roscon2025_control_workshop) + +Help us add more! + ## License From 19b3da651f9ad371570409111a8632b3572c597c Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 00:05:38 +0200 Subject: [PATCH 06/11] Fix for c++ --- src/picoros.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/picoros.c b/src/picoros.c index edfaf6e..7fc9186 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -472,15 +472,18 @@ picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* paylo opts->attachment = z_bytes_move(&tx_attachment); // Closure - z_owned_closure_reply_t callback; - z_closure(&callback, get_data_handler, get_drop_handler, client); + z_owned_closure_reply_t callback = { + ._val.call = get_data_handler, + ._val.drop = get_drop_handler, + ._val.context = client, + }; client->_in_progress = true; if ((res = z_get(z_session_loan(&s_wrapper), z_view_keyexpr_loan(&ke), "", z_closure_reply_move(&callback), opts)) != Z_OK) { _PR_LOG("Error calling %s service! Error:%d\n", client->topic.name, res); client->_in_progress = false; - z_drop(opts->attachment); - z_drop(opts->payload); + z_bytes_drop(opts->attachment); + z_bytes_drop(opts->payload); return PICOROS_ERROR; } return PICOROS_OK; From a3230663d82abdd556e721b68c416fc86b178e46 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 18:07:44 +0200 Subject: [PATCH 07/11] Add missing topic name seperator conversion --- examples/srv_client_add2ints.c | 4 ++-- examples/srv_server_add2ints.c | 2 +- src/picoros.c | 26 +++++++++++++++++++++++--- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/examples/srv_client_add2ints.c b/examples/srv_client_add2ints.c index 1f1863b..be747c3 100644 --- a/examples/srv_client_add2ints.c +++ b/examples/srv_client_add2ints.c @@ -13,7 +13,6 @@ #include #include "picoros.h" #include "picoserdes.h" -#include "zenoh-pico/system/common/platform.h" // Use command line arguments to change default values #define MODE "client" @@ -27,8 +26,9 @@ void add2_client_cb(picoros_srv_client_t* client, uint8_t* reply_data, size_t re // Example service picoros_srv_client_t add2_client = { - .node_name = "add_two_ints", + .node_name = "picoros", .topic = { + .name = "services/add2", .type = ROSTYPE_NAME(srv_AddTwoInts), .rihs_hash = ROSTYPE_HASH(srv_AddTwoInts), }, diff --git a/examples/srv_server_add2ints.c b/examples/srv_server_add2ints.c index e367b52..7623189 100644 --- a/examples/srv_server_add2ints.c +++ b/examples/srv_server_add2ints.c @@ -30,7 +30,7 @@ uint8_t srv_buf[1024]; // Example service picoros_srv_server_t add2_srv = { .topic = { - .name = "add2", + .name = "services/add2", .type = ROSTYPE_NAME(srv_AddTwoInts), .rihs_hash = ROSTYPE_HASH(srv_AddTwoInts), }, diff --git a/src/picoros.c b/src/picoros.c index 7fc9186..260130b 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -65,7 +65,17 @@ static int rmw_zenoh_node_liveliness_keyexpr(picoros_node_t* node, char* keyexpr } static int rmw_zenoh_topic_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char* keyexpr) { - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic->name, topic->type, + char topic_lv[96]; + char *str = &topic_lv[0]; + strncpy(topic_lv, topic->name, 95); + // replace / with % + while (*str) { + if (*str == '/') { + *str = '%'; + } + str++; + } + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, topic->type, topic->rihs_hash); } @@ -75,8 +85,18 @@ static int rmw_zenoh_service_keyexpr(picoros_node_t* node, rmw_topic_t* topic, c topic->type, topic->rihs_hash); } else{ - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, - topic->type, topic->rihs_hash); + char topic_lv[96]; + char *str = &topic_lv[0]; + strncpy(topic_lv, topic->name, 95); + // replace / with % + while (*str) { + if (*str == '/') { + *str = '%'; + } + str++; + } + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, topic->name, + topic->type, topic->rihs_hash); } } From 9736cfbf9e3ad6822f274d9224e83619ea755783 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 18:57:42 +0200 Subject: [PATCH 08/11] Add picoros_service_client_init. Update example topic namespaces. --- examples/odometry_listener.c | 8 ++++---- examples/odometry_publisher.c | 10 +++++----- examples/srv_client_add2ints.c | 4 +++- src/picoros.c | 33 ++++++++++++++++++++++----------- src/picoros.h | 12 ++++++++++++ 5 files changed, 46 insertions(+), 21 deletions(-) diff --git a/examples/odometry_listener.c b/examples/odometry_listener.c index af5f961..17d8f8f 100644 --- a/examples/odometry_listener.c +++ b/examples/odometry_listener.c @@ -2,10 +2,10 @@ * @file odometry_listener.c * @brief Example odometry subscriber node for picoros * @date 2025-May-27 - * + * * @details This example demonstrates a ROS subscriber node that listens to * odometry messages on the "odom" topic and prints the pose data. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ @@ -27,7 +27,7 @@ void odometry_callback(uint8_t* rx_data, size_t data_len); // Example Subscriber picoros_subscriber_t sub_odo = { .topic = { - .name = "odom", + .name = "robot/odometry", .type = ROSTYPE_NAME(ros_Odometry), .rihs_hash = ROSTYPE_HASH(ros_Odometry), }, @@ -36,7 +36,7 @@ picoros_subscriber_t sub_odo = { // Example node picoros_node_t node = { - .name = "listener", + .name = "picoros", }; void odometry_callback(uint8_t* rx_data, size_t data_len){ diff --git a/examples/odometry_publisher.c b/examples/odometry_publisher.c index 475c194..63e3528 100644 --- a/examples/odometry_publisher.c +++ b/examples/odometry_publisher.c @@ -2,13 +2,13 @@ * @file odometry_publisher.c * @brief Example odometry publisher node for picoros * @date 2025-May-27 - * + * * @details This example demonstrates a ROS publisher node that publishes * simulated odometry messages on the "odom" topic. - * + * * @copyright Copyright (c) 2025 Ubiquity Robotics *******************************************************************************/ - + #include #include #include "picoros.h" @@ -24,7 +24,7 @@ extern int picoros_parse_args(int argc, char **argv, picoros_interface_t* ifx); // Example Publisher picoros_publisher_t pub_odo = { .topic = { - .name = "odom", + .name = "robot/odometry", .type = ROSTYPE_NAME(ros_Odometry), .rihs_hash = ROSTYPE_HASH(ros_Odometry), }, @@ -32,7 +32,7 @@ picoros_publisher_t pub_odo = { // Example node picoros_node_t node = { - .name = "talker", + .name = "picoros", }; // Buffer for publication, used from this thread diff --git a/examples/srv_client_add2ints.c b/examples/srv_client_add2ints.c index be747c3..53834e4 100644 --- a/examples/srv_client_add2ints.c +++ b/examples/srv_client_add2ints.c @@ -67,6 +67,8 @@ int main(int argc, char **argv){ z_sleep_s(1); } + picoros_service_client_init(&add2_client); + int a = 0; int b = 100; while(true){ @@ -77,7 +79,7 @@ int main(int argc, char **argv){ printf("Sent service call...\n"); a++; } - z_sleep_s(1); + z_sleep_ms(100); } return 0; } diff --git a/src/picoros.c b/src/picoros.c index 260130b..573a77a 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -447,25 +447,36 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t return PICOROS_OK; } -picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* payload, size_t len){ - if (client == NULL) { return PICOROS_ERROR;} - if (client->_in_progress) { return PICOROS_NOT_READY;} - - z_result_t res; - char keyexpr[KEYEXPR_SIZE]; +picoros_res_t picoros_service_client_init(picoros_srv_client_t * client){ + if (client->_key_buf == NULL){ + client->_key_buf = z_malloc(KEYEXPR_SIZE); + } // Generate key expressions - z_view_keyexpr_t ke; if (client->topic.type != NULL) { picoros_node_t node = { .domain_id = client->node_domain_id, .name = client->node_name, }; - rmw_zenoh_service_keyexpr(&node, &client->topic, keyexpr); - z_view_keyexpr_from_str_unchecked(&ke, keyexpr); + rmw_zenoh_service_keyexpr(&node, &client->topic, client->_key_buf); + z_view_keyexpr_from_str_unchecked(&client->ke, client->_key_buf); } else { - z_view_keyexpr_from_str_unchecked(&ke, client->topic.name); + z_view_keyexpr_from_str_unchecked(&client->ke, client->topic.name); + } + return PICOROS_OK; +} + + +picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* payload, size_t len){ + if (client == NULL) { return PICOROS_ERROR;} + if (client->_in_progress) { return PICOROS_NOT_READY;} + + z_result_t res; + + // create key expression if not done before + if (client->_key_buf == NULL){ + picoros_service_client_init(client); } // Default options @@ -499,7 +510,7 @@ picoros_res_t picoros_service_call(picoros_srv_client_t * client, uint8_t* paylo }; client->_in_progress = true; - if ((res = z_get(z_session_loan(&s_wrapper), z_view_keyexpr_loan(&ke), "", z_closure_reply_move(&callback), opts)) != Z_OK) { + if ((res = z_get(z_session_loan(&s_wrapper), z_view_keyexpr_loan(&client->ke), "", z_closure_reply_move(&callback), opts)) != Z_OK) { _PR_LOG("Error calling %s service! Error:%d\n", client->topic.name, res); client->_in_progress = false; z_bytes_drop(opts->attachment); diff --git a/src/picoros.h b/src/picoros.h index c7ab696..3c34e20 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -152,6 +152,8 @@ typedef struct picoros_srv_client_s{ bool _in_progress; /**< Private flag used to limit to one ongoing request */ z_get_options_t* opts; /**< Request options, if NULL default options are used */ void* user_data; /**< User data, not used by picoros */ + z_view_keyexpr_t ke; /**< Precomputed when creating the client */ + char* _key_buf; /**< Private buffer for key expresion */ } picoros_srv_client_t; /** @} */ @@ -304,6 +306,16 @@ picoros_res_t picoros_unsubscribe(picoros_subscriber_t *sub); */ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_srv_server_t* srv); + +/** + * @brief Initialize service client with precomputed key expression. + * @param client Pointer to client instance. + * @return PICOROS_OK on success, error code otherwise + * @ingroup service_client + */ +picoros_res_t picoros_service_client_init(picoros_srv_client_t * client); + + /** * @brief Call service using service client. * @param client Pointer to client instance. Should be in scope until service call is ongoing. From b8a57c7760104d3c5e63470372edfedab42411c8 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 20:06:23 +0200 Subject: [PATCH 09/11] Fix rmw_zenoh_service_keyexpr for case without node name. --- src/picoros.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/picoros.c b/src/picoros.c index 573a77a..3814b21 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -80,22 +80,22 @@ static int rmw_zenoh_topic_keyexpr(picoros_node_t* node, rmw_topic_t* topic, cha } static int rmw_zenoh_service_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char* keyexpr) { - if (topic->name == NULL){ - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, node->name, - topic->type, topic->rihs_hash); + char topic_lv[96]; + char *str = &topic_lv[0]; + strncpy(topic_lv, topic->name, 95); + // replace / with % + while (*str) { + if (*str == '/') { + *str = '%'; + } + str++; + } + if (node->name == NULL){ + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, + topic->type, topic->rihs_hash); } else{ - char topic_lv[96]; - char *str = &topic_lv[0]; - strncpy(topic_lv, topic->name, 95); - // replace / with % - while (*str) { - if (*str == '/') { - *str = '%'; - } - str++; - } - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, topic->name, + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s%%%s/%s_/RIHS01_%s", node->domain_id, node->name, topic_lv, topic->type, topic->rihs_hash); } } From b582567620a73575466c02606d7005fd0a335092 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 20:57:35 +0200 Subject: [PATCH 10/11] Remove not needed topic name mangling. Move magic constants to defines. --- src/picoros.c | 37 +++++++++---------------------------- src/picoros.h | 4 +++- 2 files changed, 12 insertions(+), 29 deletions(-) diff --git a/src/picoros.c b/src/picoros.c index 3814b21..a57cd80 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -65,37 +65,17 @@ static int rmw_zenoh_node_liveliness_keyexpr(picoros_node_t* node, char* keyexpr } static int rmw_zenoh_topic_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char* keyexpr) { - char topic_lv[96]; - char *str = &topic_lv[0]; - strncpy(topic_lv, topic->name, 95); - // replace / with % - while (*str) { - if (*str == '/') { - *str = '%'; - } - str++; - } - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, topic->type, + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic->name, topic->type, topic->rihs_hash); } static int rmw_zenoh_service_keyexpr(picoros_node_t* node, rmw_topic_t* topic, char* keyexpr) { - char topic_lv[96]; - char *str = &topic_lv[0]; - strncpy(topic_lv, topic->name, 95); - // replace / with % - while (*str) { - if (*str == '/') { - *str = '%'; - } - str++; - } if (node->name == NULL){ - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic_lv, + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s_/RIHS01_%s", node->domain_id, topic->name, topic->type, topic->rihs_hash); } else{ - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s%%%s/%s_/RIHS01_%s", node->domain_id, node->name, topic_lv, + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s%%%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, topic->type, topic->rihs_hash); } } @@ -104,17 +84,18 @@ static int rmw_zenoh_topic_liveliness_keyexpr(picoros_node_t* node, rmw_topic_t* #if USE_NODE_GUID == 1 uint8_t* guid = node->guid; #endif - char topic_lv[96]; + char topic_lv[TOPIC_MAX_NAME]; + topic_lv[TOPIC_MAX_NAME-1] = 0; char *str = &topic_lv[0]; z_id_t id = z_info_zid(z_session_loan(&s_wrapper)); - if (strcmp(entity_str, "SS") == 0){ - // is service - snprintf(topic_lv, 95, "%s/%s", node->name, topic->name); + if (strcmp(entity_str, "SS") == 0 && node->name != NULL){ + // is service and node name is set + snprintf(topic_lv, TOPIC_MAX_NAME-1, "%s/%s", node->name, topic->name); } else{ - strncpy(topic_lv, topic->name, 95); + strncpy(topic_lv, topic->name, TOPIC_MAX_NAME-1); } // replace / with % diff --git a/src/picoros.h b/src/picoros.h index 3c34e20..74d065b 100644 --- a/src/picoros.h +++ b/src/picoros.h @@ -33,8 +33,10 @@ #include /* Exported constants --------------------------------------------------------*/ -/** @brief Maximum size for key expressions used in topic names @ingroup picoros */ +/** @brief Maximum size for key expressions used in rmw topic names @ingroup picoros */ #define KEYEXPR_SIZE 400u +/** @brief Maximum size for fully_qualified_name @ingroup picoros */ +#define TOPIC_MAX_NAME 124u /** @brief Size of RMW GID (Global Identifier) */ #define RMW_GID_SIZE 16u /** @brief Flag to enable/disable node GUID usage @ingroup picoros*/ From 46601f40d2b8c97a31fd5aaaac5fab1aa4a1f4a9 Mon Sep 17 00:00:00 2001 From: Gasper Jersin Date: Sat, 6 Sep 2025 21:01:19 +0200 Subject: [PATCH 11/11] Return / separator in service keyexpression. --- src/picoros.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/picoros.c b/src/picoros.c index a57cd80..2e61960 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -75,7 +75,7 @@ static int rmw_zenoh_service_keyexpr(picoros_node_t* node, rmw_topic_t* topic, c topic->type, topic->rihs_hash); } else{ - return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s%%%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, + return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRIu32 "/%s/%s/%s_/RIHS01_%s", node->domain_id, node->name, topic->name, topic->type, topic->rihs_hash); } }