Zenoh-pico add ROS2 Unix example using CycloneDDS CDR serializer

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**  

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)

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)
+```

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, <[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

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, <[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