/**************************************************************************** * * Copyright (c) 2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ #include "zenoh.h" #include "zenoh-pico/api/macros.h" #include "zenoh-pico/api/primitives.h" #include #include #include #include #include #include #include #include #include #include // CycloneDDS CDR Deserializer #include // Auto-generated header to all uORB <-> CDR conversions #include #define Z_PUBLISH #define Z_SUBSCRIBE extern "C" __EXPORT int zenoh_main(int argc, char *argv[]); void toCamelCase(char *input) { int capitalizeNext = 1; // Capitalize the first letter int j = 0; for (int i = 0; input[i] != '\0'; i++) { if (input[i] == '_') { capitalizeNext = 1; // Next letter should be capitalized } else { if (capitalizeNext && isalpha(input[i])) { input[j++] = toupper(input[i]); capitalizeNext = 0; } else { input[j++] = input[i]; } } } input[j] = '\0'; // Null-terminate the input string } ZENOH::ZENOH(): ModuleParams(nullptr) { } ZENOH::~ZENOH() { } int ZENOH::generate_rmw_zenoh_node_liveliness_keyexpr(const z_id_t *id, char *keyexpr) { return snprintf(keyexpr, KEYEXPR_SIZE, "@ros2_lv/0/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/0/NN/%%/%%/" "px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", id->id[0], id->id[1], id->id[2], id->id[3], id->id[4], id->id[5], id->id[6], id->id[7], id->id[8], id->id[9], id->id[10], id->id[11], id->id[12], id->id[13], id->id[14], id->id[15], px4_guid[0], px4_guid[1], px4_guid[2], px4_guid[3], px4_guid[4], px4_guid[5], px4_guid[6], px4_guid[7], px4_guid[8], px4_guid[9], px4_guid[10], px4_guid[11], px4_guid[12], px4_guid[13], px4_guid[14], px4_guid[15]); } int ZENOH::generate_rmw_zenoh_topic_keyexpr(const char *topic, const uint8_t *rihs_hash, char *type, char *keyexpr) { const char *type_name = getTypeName(type); if (type_name) { strcpy(type, type_name); toCamelCase(type); // Convert uORB type to camel case return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%s/" KEYEXPR_MSG_NAME "%s_/RIHS01_" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x", _zenoh_domain_id.get(), topic, type, rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3], rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7], rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11], rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15], rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19], rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23], rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27], rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31] ); } return -1; } int ZENOH::generate_rmw_zenoh_topic_liveliness_keyexpr(const z_id_t *id, const char *topic, const uint8_t *rihs_hash, char *type_camel_case, char *keyexpr, const char *entity_str) { // NOT REALLY COMPLIANT WITH RMW_ZENOH_CPP but get's the job done // TODO build a correct keyexpr return snprintf(keyexpr, KEYEXPR_SIZE, "@ros2_lv/%" PRId32 "/" "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/" "0/11/%s/%%/%%/px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/%%%s/" KEYEXPR_MSG_NAME "%s_/RIHS01_" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x" "%02x%02x%02x%02x%02x%02x%02x%02x" "/::,7:,:,:,,", _zenoh_domain_id.get(), id->id[0], id->id[1], id->id[2], id->id[3], id->id[4], id->id[5], id->id[6], id->id[7], id->id[8], id->id[9], id->id[10], id->id[11], id->id[12], id->id[13], id->id[14], id->id[15], entity_str, px4_guid[0], px4_guid[1], px4_guid[2], px4_guid[3], px4_guid[4], px4_guid[5], px4_guid[6], px4_guid[7], px4_guid[8], px4_guid[9], px4_guid[10], px4_guid[11], px4_guid[12], px4_guid[13], px4_guid[14], px4_guid[15], topic, type_camel_case, rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3], rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7], rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11], rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15], rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19], rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23], rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27], rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31] ); } int ZENOH::setupSession() { char mode[NET_MODE_SIZE]; char locator[NET_LOCATOR_SIZE]; int ret = 0; _config.getNetworkConfig(mode, locator); z_owned_config_t config; z_config_default(&config); zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode); if (locator[0] != 0) { zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator); } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT); } PX4_INFO("Opening session..."); ret = z_open(&s, z_move(config), NULL); if (ret < 0) { if (ret == _Z_ERR_TRANSPORT_OPEN_FAILED) { PX4_ERR("Unable to open session, make sure zenohd is running on %s", locator); } else if (ret == _Z_ERR_SCOUT_NO_RESULTS) { PX4_ERR("Unable to open session, scout no results"); } else { PX4_ERR("Unable to open session, ret: %d", ret); } return ret; } return ret; } int ZENOH::setupTopics(px4_pollfd_struct_t *pfds) { char keyexpr[KEYEXPR_SIZE]; int i; int ret = 0; #ifndef BOARD_HAS_NO_UUID board_get_px4_guid(px4_guid); #else // TODO Fill ID with something reasonable px4_guid[0] = 0xAA; px4_guid[1] = 0xBB; px4_guid[2] = 0xCC; #endif // Start read and lease tasks for zenoh-pico if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) { PX4_ERR("Unable to start read and lease tasks"); z_drop(z_move(s)); return -EINVAL; } #ifdef CONFIG_ZENOH_RMW_LIVELINESS z_id_t self_id = z_info_zid(z_loan(s)); if (generate_rmw_zenoh_node_liveliness_keyexpr(&self_id, keyexpr)) { z_view_keyexpr_t ke; if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) { printf("%s is not a valid key expression\n", keyexpr); return 1; } z_owned_liveliness_token_t token; if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) { printf("Unable to create liveliness token!\n"); return -1; } } #endif #ifdef Z_SUBSCRIBE _zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count); { char topic[TOPIC_INFO_SIZE]; char type[TOPIC_INFO_SIZE]; for (i = 0; i < _sub_count; i++) { _config.getSubscriberMapping(topic, type); _zenoh_subscribers[i] = genSubscriber(type); const uint8_t *rihs_hash = getRIHS01_Hash(type); if (rihs_hash != NULL && _zenoh_subscribers[i] != 0 && generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) { _zenoh_subscribers[i]->declare_subscriber(s, keyexpr); #ifdef CONFIG_ZENOH_RMW_LIVELINESS if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MS") > 0) { z_view_keyexpr_t ke; if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) { printf("%s is not a valid key expression\n", keyexpr); return -1; } z_owned_liveliness_token_t token; if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) { printf("Unable to create liveliness token!\n"); return -1; } } #endif } else { PX4_ERR("Could not create a subscriber for type %s", type); } if (_config.getSubscriberMapping(topic, type) < 0) { PX4_WARN("Subscriber mapping parsing error"); } } } #endif #ifdef Z_PUBLISH _zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *)); { char topic[TOPIC_INFO_SIZE]; char type[TOPIC_INFO_SIZE]; for (i = 0; i < _pub_count; i++) { _config.getPublisherMapping(topic, type); _zenoh_publishers[i] = genPublisher(type); const uint8_t *rihs_hash = getRIHS01_Hash(type); if (rihs_hash && _zenoh_publishers[i] != 0 && generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) { _zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid); _zenoh_publishers[i]->setPollFD(&pfds[i]); #ifdef CONFIG_ZENOH_RMW_LIVELINESS if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MP") > 0) { z_view_keyexpr_t ke; if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) { printf("%s is not a valid key expression\n", keyexpr); return -1; } z_owned_liveliness_token_t token; if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) { printf("Unable to create liveliness token!\n"); return -1; } } #endif } else { PX4_ERR("Could not create a publisher for type %s", type); } } if (_config.getSubscriberMapping(topic, type) < 0) { PX4_WARN("Publisher mapping parsing error"); } } #endif return ret; } void ZENOH::run() { int8_t ret; int i; _pub_count = _config.getPubCount(); _sub_count = _config.getSubCount(); px4_pollfd_struct_t pfds[_pub_count]; if (setupSession() < 0) { PX4_ERR("Failed to setup Zenoh session"); return; } PX4_INFO("Starting reading/writing tasks..."); if (setupTopics(pfds) < 0) { PX4_ERR("Failed to setup topics"); return; } if (_pub_count == 0) { // Nothing to publish but we don't want to stop this thread while (!should_exit()) { usleep(1000); } } while (!should_exit()) { int pret = px4_poll(pfds, _pub_count, 100); if (pret == 0) { //PX4_INFO("Zenoh poll timeout\n"); } else { for (i = 0; i < _pub_count; i++) { if (pfds[i].revents & POLLIN) { ret = _zenoh_publishers[i]->update(); if (ret < 0) { PX4_WARN("Publisher error %i", ret); } } } } } // Exiting cleaning up publisher and subscribers for (i = 0; i < _sub_count; i++) { if (_zenoh_subscribers[i]) { delete _zenoh_subscribers[i]; } } free(_zenoh_subscribers); for (i = 0; i < _pub_count; i++) { if (_zenoh_publishers[i]) { delete _zenoh_publishers[i]; } } free(_zenoh_publishers); // Stop read and lease tasks for zenoh-pico zp_stop_read_task(z_session_loan_mut(&s)); zp_stop_lease_task(z_session_loan_mut(&s)); z_drop(z_session_move(&s)); exit_and_cleanup(); } int ZENOH::custom_command(int argc, char *argv[]) { if (argc > 0 && strcmp("config", argv[0]) == 0) { Zenoh_Config z_config; if (z_config.cli(argc, argv) == 0) { return 0; } } return print_usage("Unrecognized command."); } int ZENOH::print_usage(const char *reason) { if (reason) { printf("%s\n\n", reason); } PRINT_MODULE_USAGE_NAME("zenoh", "driver"); PRINT_MODULE_DESCRIPTION(R"DESC_STR( ### Description Zenoh demo bridge )DESC_STR"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND("status"); PRINT_MODULE_USAGE_COMMAND("config"); PX4_INFO_RAW(" addpublisher Publish uORB topic to Zenoh\n"); PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n"); return 0; } int ZENOH::print_status() { PX4_INFO("running"); PX4_INFO("Publishers"); for (int i = 0; i < _pub_count; i++) { if (_zenoh_publishers[i]) { _zenoh_publishers[i]->print(); } } PX4_INFO("Subscribers"); for (int i = 0; i < _sub_count; i++) { if (_zenoh_subscribers[i]) { _zenoh_subscribers[i]->print(); } } return 0; } int ZENOH::task_spawn(int argc, char *argv[]) { int task_id = px4_task_spawn_cmd( "zenoh", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, &run_trampoline, argv ); if (task_id < 0) { return -errno; } else { _task_id = task_id; return 0; } } ZENOH *ZENOH::instantiate(int argc, char *argv[]) { return new ZENOH(); } int zenoh_main(int argc, char *argv[]) { return ZENOH::main(argc, argv); }