eclipse-zenoh · PetervdPerk-NXP · Feb 8, 2023
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -11,7 +11,7 @@
 # Contributors:
 # ZettaScale Zenoh Team, <[email protected]>
 #
-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
@@ -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
@@ -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
@@ -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, <[email protected]>
+//
+
+#ifdef ZENOH_LINUX
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+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
@@ -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, <[email protected]>
+//
+
+#ifdef ZENOH_LINUX
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdarg.h>
+
+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
diff --git a/examples/unix/ros2/z_pub_ros2.c 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, <[email protected]>
+//   Peter van der Perk, <[email protected]>
+//
+
+#include <ctype.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <zenoh-pico.h>
+#include <rcl_interfaces/msg/Log.h>
+
+// CycloneDDS CDR Deserializer
+#include <dds/cdr/dds_cdrstream.h>
+
+// 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;
+}