diff --git a/ROS2/README.md b/ROS2/README.md index 59cd0e7..66f5073 100644 --- a/ROS2/README.md +++ b/ROS2/README.md @@ -12,6 +12,9 @@ * ROS2 gyroscope-based teleop from a microcontroller using Eclipse zenoh-pico (more details in [this blog](https://zenoh.io/blog/2021-11-09-ros2-zenoh-pico/)): * **[zenoh-pico-teleop-gyro](./zenoh-pico-teleop-gyro)**: in **C** + * Simple example on how to use CycloneDDS CDR library to do DDS IDL serialization/deserialization on top of Zenoh-Pico. + * **[zenoh-pico-cyclonedds-cdr-message-log](./zenoh-pico-cyclonedds-cdr-message-log)**: in **C** + ## Other related demos and blogs * **A Tele-operation demo deployed with Eclipse fog05 and Eclipse zenoh** diff --git a/ROS2/zenoh-pico-cyclonedds-cdr-message-log/CMakeLists.txt b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/CMakeLists.txt new file mode 100644 index 0000000..a06877a --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.19) + +# This is your project statement. You should always list languages; +# Listing the version is nice here since it sets lots of useful variables +project( + zenoh-ros2 + VERSION 1.0 + LANGUAGES C) + +# Platform information for zenoh +if(CMAKE_SYSTEM_NAME MATCHES "Linux") + add_definitions(-DZENOH_LINUX) + set(JNI_PLATFORM_NAME "linux") +else() + message(FATAL_ERROR "zenoh-pico is not yet available on ${CMAKE_SYSTEM_NAME} platform") + return() +endif() + +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) +include_directories (${cyclonedds_BINARY_DIR}/src/core/include/) +include_directories (${cyclonedds_BINARY_DIR}/src/ddsrt/include/) + +# Zenoh-Pico +include(FetchContent) +FetchContent_Declare(zenoh-pico + GIT_REPOSITORY "https://github.com/eclipse-zenoh/zenoh-pico" + GIT_TAG "origin/master" + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/zenoh-pico" +) +FetchContent_MakeAvailable(zenoh-pico) +set(ZENOHPICO_DIR ${CMAKE_CURRENT_BINARY_DIR}/zenoh-pico) +include_directories (${ZENOHPICO_DIR}/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_dependencies(z_pub_ros2 zenohpico) + +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) +add_dependencies(z_sub_ros2 zenohpico) + +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES /usr/local/lib ${CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES}) +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 zenohpico) +target_link_libraries(z_pub_ros2 zenohpico) diff --git a/ROS2/zenoh-pico-cyclonedds-cdr-message-log/README.md b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/README.md new file mode 100644 index 0000000..cdd0df7 --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/README.md @@ -0,0 +1,64 @@ +# 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 + +```bash + $ mkdir build + $ cmake .. -DROS_DISTRO=galactic # For other ROS Distro's use a different name + $ make +``` + +## Demo setup + +```bash + $ 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 + +```bash + $ ./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 + +```bash + $ 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/ROS2/zenoh-pico-cyclonedds-cdr-message-log/hal/heap.c b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/hal/heap.c new file mode 100644 index 0000000..a018c0a --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/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/ROS2/zenoh-pico-cyclonedds-cdr-message-log/hal/log.c b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/hal/log.c new file mode 100644 index 0000000..fa0b69b --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/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/ROS2/zenoh-pico-cyclonedds-cdr-message-log/z_pub_ros2.c b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/z_pub_ros2.c new file mode 100644 index 0000000..2e3fba4 --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/z_pub_ros2.c @@ -0,0 +1,146 @@ +// +// 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 *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 == '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 + rcl_interfaces_msg_Log msg; + 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); + + 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; + + struct timespec ts; + 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 == true) { + 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/ROS2/zenoh-pico-cyclonedds-cdr-message-log/z_sub_ros2.c b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/z_sub_ros2.c new file mode 100644 index 0000000..9ed3074 --- /dev/null +++ b/ROS2/zenoh-pico-cyclonedds-cdr-message-log/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; +}