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/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/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/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 new file mode 100644 index 0000000..53834e4 --- /dev/null +++ b/examples/srv_client_add2ints.c @@ -0,0 +1,85 @@ +/******************************************************************************* + * @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" +#include "picoserdes.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(picoros_srv_client_t* client, uint8_t* reply_data, size_t reply_size, bool error); + +// Example service +picoros_srv_client_t add2_client = { + .node_name = "picoros", + .topic = { + .name = "services/add2", + .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(picoros_srv_client_t* client, 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); + } + + picoros_service_client_init(&add2_client); + + 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_ms(100); + } + return 0; +} diff --git a/examples/srv_add2ints.c b/examples/srv_server_add2ints.c similarity index 88% rename from examples/srv_add2ints.c rename to examples/srv_server_add2ints.c index 72f1056..7623189 100644 --- a/examples/srv_add2ints.c +++ b/examples/srv_server_add2ints.c @@ -1,14 +1,14 @@ /******************************************************************************* - * @file srv_add2ints.c + * @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,15 +22,15 @@ 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]; // Example service -picoros_service_t add2_srv = { +picoros_srv_server_t add2_srv = { .topic = { - .name = "add2", + .name = "services/add2", .type = ROSTYPE_NAME(srv_AddTwoInts), .rihs_hash = ROSTYPE_HASH(srv_AddTwoInts), }, @@ -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/readme.md b/readme.md index 43e84f8..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 @@ -56,7 +59,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. @@ -111,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 diff --git a/src/picoparams.c b/src/picoparams.c index dd93c67..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 *******************************************************************************/ @@ -14,21 +14,21 @@ /* 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 ------------------------------------------------------------*/ /* 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_service_t* s = (picoros_service_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 70ee302..94d8327 100644 --- a/src/picoros.c +++ b/src/picoros.c @@ -2,24 +2,22 @@ * @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 *******************************************************************************/ /* 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,25 +70,32 @@ 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 (node->name == NULL){ + 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->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) { #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 % @@ -145,8 +150,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; @@ -160,7 +165,7 @@ static void srv_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 @@ -191,7 +196,48 @@ 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){ + 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){ + if (ctx == NULL){ + return; + } + 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(client, raw_data, raw_data_len, error); + z_free(raw_data); +} /* Public functions ----------------------------------------------------------*/ @@ -345,7 +391,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]; @@ -364,7 +410,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); @@ -383,6 +429,83 @@ picoros_res_t picoros_service_declare(picoros_node_t* node, picoros_service_t* s return PICOROS_OK; } + +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 + 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, client->_key_buf); + z_view_keyexpr_from_str_unchecked(&client->ke, client->_key_buf); + } + else { + 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 + 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 = RMW_GID_SIZE, + .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 = { + ._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(&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); + z_bytes_drop(opts->payload); + return PICOROS_ERROR; + } + 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 29df905..85f8a19 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 *******************************************************************************/ @@ -28,11 +28,15 @@ /** @} */ /* Exported includes ---------------------------------------------------------*/ + #include +#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*/ @@ -40,7 +44,7 @@ /* Exported types ------------------------------------------------------------*/ - /** +/** * @defgroup rmw RMW support * @ingroup picoros * @{ @@ -60,18 +64,22 @@ 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; /** @} */ - /** +/** * @defgroup service_server Service server * @ingroup picoros * @{ */ + +/* Forward declaration */ +struct picoros_srv_server_s; + /** * @brief Service reply data structure */ @@ -83,32 +91,75 @@ 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 - */ -typedef picoros_service_reply_t (*picoros_service_cb_t)( - uint8_t* request_data, - size_t reqest_size, - void* user_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)( + 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 { - 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; +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, not used by picoros */ + picoros_srv_server_cb_t user_callback; /**< User callback for service handling */ +} picoros_srv_server_t; /** @} */ +/** + * @defgroup service_client Service client + * @ingroup picoros + * @{ + */ + +/* Forward declaration */ +struct picoros_srv_client_s; + +/** + * @brief Callback function type for service reply handling + * @return void + */ +typedef void (*picoros_srv_client_cb_t)( + 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 Callback function type for service reply dropped handling + * @param client picoros_srv_client_t that started the request + * @return void + */ +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 */ + z_view_keyexpr_t ke; /**< Precomputed when creating the client */ + char* _key_buf; /**< Private buffer for key expresion */ +} picoros_srv_client_t; + +/** @} */ + /** * @brief Publisher structure for Pico-ROS @ingroup picoros */ @@ -127,14 +178,13 @@ typedef struct { * @ingroup picoros * @{ */ + /** * @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 */ ); /** @@ -165,12 +215,12 @@ typedef struct { /** @} */ - /** * @defgroup interface Network interface * @ingroup picoros * @{ */ + /** * @brief Network interface configuration */ @@ -181,8 +231,6 @@ typedef struct { /** @} */ - - /** * @brief Result codes for Pico-ROS operations @ingroup picoros */ @@ -213,7 +261,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); /** @@ -259,7 +307,35 @@ 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 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. + * @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 } 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