From c6b7f07914871dd3c1436717ee959dc0fb169784 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 8 Feb 2023 09:24:32 +0100 Subject: [PATCH] Zenoh-pico add ROS2 Unix example using CycloneDDS CDR serializer --- CMakeLists.txt | 6 +- examples/unix/ros2/CMakeLists.txt | 77 +++++++++++++++ examples/unix/ros2/README.md | 52 ++++++++++ examples/unix/ros2/hal/heap.c | 60 ++++++++++++ examples/unix/ros2/hal/log.c | 32 +++++++ examples/unix/ros2/z_pub_ros2.c | 151 ++++++++++++++++++++++++++++++ examples/unix/ros2/z_sub_ros2.c | 127 +++++++++++++++++++++++++ 7 files changed, 504 insertions(+), 1 deletion(-) create mode 100644 examples/unix/ros2/CMakeLists.txt create mode 100644 examples/unix/ros2/README.md create mode 100644 examples/unix/ros2/hal/heap.c create mode 100644 examples/unix/ros2/hal/log.c create mode 100644 examples/unix/ros2/z_pub_ros2.c create mode 100644 examples/unix/ros2/z_sub_ros2.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 780c3f129..6c34e374b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ # Contributors: # ZettaScale Zenoh Team, # -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.19) project(libzenohpico VERSION 0.7.0.1 LANGUAGES C) @@ -201,6 +201,10 @@ if(BUILD_EXAMPLES) add_executable(z_queryable ${PROJECT_SOURCE_DIR}/examples/unix/c11/z_queryable.c) add_executable(z_info ${PROJECT_SOURCE_DIR}/examples/unix/c11/z_info.c) add_executable(z_scout ${PROJECT_SOURCE_DIR}/examples/unix/c11/z_scout.c) + + if(ROS_DISTRO) # When ROS_DISTRO is we build the ROS2 IDL/CDR examples + add_subdirectory(${PROJECT_SOURCE_DIR}/examples/unix/ros2) + endif() endif() target_link_libraries(z_put ${Libname}) diff --git a/examples/unix/ros2/CMakeLists.txt b/examples/unix/ros2/CMakeLists.txt new file mode 100644 index 000000000..7b718cc8a --- /dev/null +++ b/examples/unix/ros2/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.19) + + +if(DEFINED ENV{ROS_DISTRO}) + message(FATAL_ERROR "ROS2 environment sourced, you shouldn't source ROS") +endif() + +if(NOT DEFINED ROS_DISTRO) + message(FATAL_ERROR "No ROS_DISTRO defined") +else() + message("-- Using ROS ${ROS_DISTRO}") +endif() + +# ROS Paths +set(ROS_PATH "/opt/ros/${ROS_DISTRO}/share") +set(RCL_INTERFACES_PATH "${ROS_PATH}/rcl_interfaces") +set(BUILTIN_INTERFACES_PATH "${ROS_PATH}/builtin_interfaces") + +# CycloneDDS config + +set(BUILD_IDLC_ONLY YES) + +# CycloneDDS IDL +include(FetchContent) +FetchContent_Declare(cyclonedds + GIT_REPOSITORY "https://github.com/eclipse-cyclonedds/cyclonedds" + GIT_TAG "origin/master" + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/cyclonedds" +) + +FetchContent_MakeAvailable(cyclonedds) + +set(CYCLONEDDS_DIR ${CMAKE_CURRENT_BINARY_DIR}/cyclonedds) +include("${CYCLONEDDS_DIR}/cmake/Modules/Generate.cmake") + +include("${RCL_INTERFACES_PATH}/cmake/rosidl_cmake-extras.cmake") +include("${BUILTIN_INTERFACES_PATH}/cmake/rosidl_cmake-extras.cmake") + +foreach(_idl ${rcl_interfaces_IDL_FILES}) + list(APPEND IDL_FILES "${RCL_INTERFACES_PATH}/${_idl}") +endforeach() + +foreach(_idl ${builtin_interfaces_IDL_FILES}) + list(APPEND IDL_FILES "${BUILTIN_INTERFACES_PATH}/${_idl}") +endforeach() + +idlc_generate(TARGET rcl_interfaces_msgs FILES ${IDL_FILES} INCLUDES ${ROS_PATH} BASE_DIR ${ROS_PATH} WARNINGS no-implicit-extensibility) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +include_directories (${CMAKE_BINARY_DIR}) +include_directories (${CYCLONEDDS_DIR}/src/core/cdr/include) +include_directories (${CYCLONEDDS_DIR}/src/ddsrt/include) +include_directories (${CYCLONEDDS_DIR}/src/core/ddsc/include) + +# Adding something we can run - Output name matches target name +add_executable(z_pub_ros2 + z_pub_ros2.c + hal/heap.c + hal/log.c + rcl_interfaces/msg/Log.c + builtin_interfaces/msg/Time.c + ${CYCLONEDDS_DIR}/src/core/cdr/src/dds_cdrstream.c + ${CYCLONEDDS_DIR}/src/ddsrt/src/bswap.c) + +add_executable(z_sub_ros2 + z_sub_ros2.c + hal/heap.c + hal/log.c + rcl_interfaces/msg/Log.c + builtin_interfaces/msg/Time.c + ${CYCLONEDDS_DIR}/src/core/cdr/src/dds_cdrstream.c + ${CYCLONEDDS_DIR}/src/ddsrt/src/bswap.c) + +target_compile_definitions(z_sub_ros2 PRIVATE "DDS_LOG=0") +target_compile_definitions(z_pub_ros2 PRIVATE "DDS_LOG=0") +target_link_libraries(z_sub_ros2 ${Libname}) +target_link_libraries(z_pub_ros2 ${Libname}) diff --git a/examples/unix/ros2/README.md b/examples/unix/ros2/README.md new file mode 100644 index 000000000..63582e870 --- /dev/null +++ b/examples/unix/ros2/README.md @@ -0,0 +1,52 @@ +# Zenoh-Pico ROS2 Example +This example shows on how to interface a zenoh-pico subscriber/publisher with the ROS2 DDS domain. It utilizes the CycloneDDS CDR library to do DDS IDL serialization/deserialization. +# Demo +Requirements + + - A ROS2 Distribution for example [ROS2 galactic](https://docs.ros.org/en/galactic/Installation.html). + - The Zenoh router + - [Zenoh DDS plugin](https://github.com/eclipse-zenoh/zenoh-plugin-dds#how-to-install-it) to bridge Zenoh and DDS + +Compilation + + $ cd /path/to/zenoh-pico + $ mkdir build + $ cmake .. -DROS_DISTRO=galactic # For other ROS Distro's use a different name + $ make + Demo setup + + + $ zenohd & # Starts Zenoh router + $ zenoh-bridge-dds -m client & # Starts Zenoh DDS bridge + $ source /opt/ros/galactic/setup.bash # For other ROS Distro's use a different name + $ ros2 topic echo /zenoh_log_test rcl_interfaces/msg/Log +On a second terminal start the Zenoh-pico publisher + + $ cd /path/to/zenoh-pico + $ ./build/examples/z_pub_ros2 + Opening session... + Declaring publisher for 'rt/zenoh_log_test'... + Putting Data ('rt/zenoh_log_test')... + Putting Data ('rt/zenoh_log_test')... +The first terminal should show the ROS2 output + + --- + stamp: + sec: 1675777460 + nanosec: 832124177 + level: 20 + name: zenoh_log_test + msg: Hello from Zenoh to ROS2 encoded with CycloneDDS dds_cdrstream serializer + file: z_pub_ros2.c + function: z_publisher_put + line: 138 +On a third terminal you can start the Zenoh-pico subscriber + + $ cd /path/to/zenoh-pico + $ ./build/examples/z_sub_ros2 + Opening session... + Declaring Subscriber on 'rt/zenoh_log_test'... + Enter 'q' to quit... + >> [Subscriber] Received ('rt/zenoh_log_test' size '160') + >> Time(sec=1675777461, nanosec=832501494) + >> Log(level=20, name='zenoh_log_test', msg='Hello from Zenoh to ROS2 encoded with CycloneDDS dds_cdrstream serializer', file='z_pub_ros2.c', function='z_publisher_put', line=138) diff --git a/examples/unix/ros2/hal/heap.c b/examples/unix/ros2/hal/heap.c new file mode 100644 index 000000000..a018c0a17 --- /dev/null +++ b/examples/unix/ros2/hal/heap.c @@ -0,0 +1,60 @@ +// +// Copyright (c) 2022 NXP +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// Peter van der Perk, +// + +#ifdef ZENOH_LINUX + +#include +#include +#include + +void *ddsrt_calloc(size_t nitems, size_t size) { + return calloc(nitems, size); +} + +void *dds_realloc(void *ptr, size_t size) { + return realloc(ptr, size); +} + +void *ddsrt_realloc(void *ptr, size_t size) { + return realloc(ptr, size); +} + +void *ddsrt_malloc(size_t size) { + return malloc(size); +} + +void *ddsrt_malloc_s(size_t size) { + return malloc(size ? size : 1); +} + +void *ddsrt_memdup(const void *src, size_t n) { + void *dest = NULL; + + if (n != 0 && (dest = ddsrt_malloc_s(n)) != NULL) { + memcpy(dest, src, n); + } + + return dest; +} + + +void ddsrt_free(void *ptr) { + free(ptr); +} + +void dds_free(void *ptr) { + free(ptr); +} + +#endif diff --git a/examples/unix/ros2/hal/log.c b/examples/unix/ros2/hal/log.c new file mode 100644 index 000000000..fa0b69bfe --- /dev/null +++ b/examples/unix/ros2/hal/log.c @@ -0,0 +1,32 @@ +// +// Copyright (c) 2022 NXP +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// Peter van der Perk, +// + +#ifdef ZENOH_LINUX + +#include +#include +#include +#include + +void dds_log (uint32_t cat, const char *file, uint32_t line, const char *func, const char *fmt, ...) +{ +#ifdef DEBUG + va_list ap; + va_start (ap, fmt); + printf(fmt, ap); + va_end (ap); +#endif +} + +#endif \ No newline at end of file diff --git a/examples/unix/ros2/z_pub_ros2.c b/examples/unix/ros2/z_pub_ros2.c new file mode 100644 index 000000000..644e77eba --- /dev/null +++ b/examples/unix/ros2/z_pub_ros2.c @@ -0,0 +1,151 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// Copyright (c) 2022 NXP +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// Peter van der Perk, +// + +#include +#include +#include +#include +#include +#include +#include + +// CycloneDDS CDR Deserializer +#include + +// CDR Xtypes header {0x00, 0x01} indicates it's Little Endian (CDR_LE representation) +const uint8_t ros2_header[4] = {0x00, 0x01, 0x00, 0x00}; + +const size_t alloc_size = 4096; // Abitrary size + +int main(int argc, char **argv) { + const char *keyexpr = "rt/zenoh_log_test"; + const char *value = "Pub from Pico IDL!"; + const char *mode = "client"; + char *locator = NULL; + rcl_interfaces_msg_Log msg; + + int opt; + while ((opt = getopt(argc, argv, "k:v:e:m:")) != -1) { + switch (opt) { + case 'k': + keyexpr = optarg; + break; + case 'v': + value = optarg; + break; + case 'e': + locator = optarg; + break; + case 'm': + mode = optarg; + break; + case '?': + if (optopt == 'k' || optopt == 'v' || optopt == 'e' || optopt == 'm') { + fprintf(stderr, "Option -%c requires an argument.\n", optopt); + } else { + fprintf(stderr, "Unknown option `-%c'.\n", optopt); + } + return 1; + default: + return -1; + } + } + + // Set HelloWorld IDL message + msg.stamp.sec = 0; + msg.stamp.nanosec = 0; + msg.level = 20; + msg.name = "zenoh_log_test"; + msg.msg = "Hello from Zenoh to ROS2 encoded with CycloneDDS dds_cdrstream serializer"; + msg.function = "z_publisher_put"; + msg.file = "z_pub_ros2.c"; + msg.line = 138; + + z_owned_config_t config = z_config_default(); + zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + if (locator != NULL) { + zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + } + + printf("Opening session...\n"); + z_owned_session_t s = z_open(z_config_move(&config)); + if (!z_session_check(&s)) { + printf("Unable to open session!\n"); + return -1; + } + + // Start read and lease tasks for zenoh-pico + if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + printf("Unable to start read and lease tasks"); + return -1; + } + + printf("Declaring publisher for '%s'...\n", keyexpr); + z_owned_publisher_t pub = z_declare_publisher(z_session_loan(&s), z_keyexpr(keyexpr), NULL); + if (!z_publisher_check(&pub)) { + printf("Unable to declare publisher for key expression!\n"); + return -1; + } + + // Setup ostream for serializer + dds_ostream_t os; + struct dds_cdrstream_desc desc; + + // Allocate buffer for serialized message + uint8_t *buf = malloc(alloc_size); + + struct timespec ts; + + + for (int idx = 0; 1; ++idx) { + sleep(1); + printf("Putting Data ('%s')...\n", keyexpr); + + // Add ROS2 header + memcpy(buf, ros2_header, sizeof(ros2_header)); + + os.m_buffer = buf; + os.m_index = sizeof(ros2_header); // Offset for CDR Xtypes header + os.m_size = alloc_size; + os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2; + + timespec_get(&ts, TIME_UTC); + msg.stamp.sec = ts.tv_sec; + msg.stamp.nanosec = ts.tv_nsec; + + dds_cdrstream_desc_from_topic_desc (&desc, &rcl_interfaces_msg_Log_desc); + + // Do serialization + bool ret = dds_stream_write_sampleLE ((dds_ostreamLE_t *) &os, (void*)&msg, &desc); + dds_cdrstream_desc_fini (&desc); + + if(ret) { + z_publisher_put_options_t options = z_publisher_put_options_default(); + options.encoding = z_encoding(Z_ENCODING_PREFIX_TEXT_PLAIN, NULL); + z_publisher_put(z_publisher_loan(&pub), (const uint8_t *)buf, os.m_index, &options); + } + } + + z_undeclare_publisher(z_publisher_move(&pub)); + + // Stop read and lease tasks for zenoh-pico + zp_stop_read_task(z_session_loan(&s)); + zp_stop_lease_task(z_session_loan(&s)); + + z_close(z_session_move(&s)); + + return 0; +} diff --git a/examples/unix/ros2/z_sub_ros2.c b/examples/unix/ros2/z_sub_ros2.c new file mode 100644 index 000000000..e3de989db --- /dev/null +++ b/examples/unix/ros2/z_sub_ros2.c @@ -0,0 +1,127 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// Copyright (c) 2022 NXP +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// Peter van der Perk, +// + +#include +#include +#include +#include +#include +#include +#include + +// CycloneDDS CDR Deserializer +#include + +void idl_deser (unsigned char *buf, uint32_t sz, void *obj, const dds_topic_descriptor_t *desc) +{ + dds_istream_t is = { .m_buffer = buf, .m_index = 0, .m_size = sz, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 }; + dds_stream_read (&is, obj, desc->m_ops); +} + +void data_handler(const z_sample_t *sample, void *arg) { + (void)(arg); + + z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_loan(keystr), (int)sample->payload.len); + z_drop(z_move(keystr)); + + // Approximate amount of memory needed to decode incoming message + // We do this so we only have to allocate once to map this easier to smaller microcontrollers + size_t decoded_size_approx = sizeof (rcl_interfaces_msg_Log) + sample->payload.len; + + void *msgData = malloc(decoded_size_approx); + rcl_interfaces_msg_Log* msg = (rcl_interfaces_msg_Log*)msgData; + // Deserialize Msg + idl_deser(((char *)sample->payload.start+4), (int)sample->payload.len, msgData, &rcl_interfaces_msg_Log_desc); + + printf(">> Time(sec=%d, nanosec=%d)\n", msg->stamp.sec, msg->stamp.nanosec); + printf(">> Log(level=%d, name='%s', msg='%s', file='%s', function='%s', line=%d)\n", msg->level, msg->name, msg->msg, msg->file, msg->function, msg->line); +} + +int main(int argc, char **argv) { + const char *keyexpr = "rt/zenoh_log_test"; + const char *mode = "client"; + char *locator = NULL; + + int opt; + while ((opt = getopt(argc, argv, "k:e:m:")) != -1) { + switch (opt) { + case 'k': + keyexpr = optarg; + break; + case 'e': + locator = optarg; + break; + case 'm': + mode = optarg; + break; + case '?': + if (optopt == 'k' || optopt == 'e' || optopt == 'm') { + fprintf(stderr, "Option -%c requires an argument.\n", optopt); + } else { + fprintf(stderr, "Unknown option `-%c'.\n", optopt); + } + return 1; + default: + return -1; + } + } + + z_owned_config_t config = z_config_default(); + zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + if (locator != NULL) { + zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + } + + printf("Opening session...\n"); + z_owned_session_t s = z_open(z_config_move(&config)); + if (!z_session_check(&s)) { + printf("Unable to open session!\n"); + return -1; + } + + // Start read and lease tasks for zenoh-pico + if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + printf("Unable to start read and lease tasks"); + return -1; + } + + z_owned_closure_sample_t callback = z_closure_sample(data_handler, NULL, NULL); + printf("Declaring Subscriber on '%s'...\n", keyexpr); + z_owned_subscriber_t sub = + z_declare_subscriber(z_session_loan(&s), z_keyexpr(keyexpr), z_closure_sample_move(&callback), NULL); + if (!z_subscriber_check(&sub)) { + printf("Unable to declare subscriber.\n"); + return -1; + } + + printf("Enter 'q' to quit...\n"); + char c = '\0'; + while (c != 'q') { + fflush(stdin); + scanf("%c", &c); + } + + z_undeclare_subscriber(z_subscriber_move(&sub)); + + // Stop read and lease tasks for zenoh-pico + zp_stop_read_task(z_session_loan(&s)); + zp_stop_lease_task(z_session_loan(&s)); + + z_close(z_session_move(&s)); + + return 0; +}