commit c38407bbd86baf769399e76a2fe030d9fd11c7ba Author: James Pace Date: Tue Jul 21 23:10:23 2026 -0400 Init commit. diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..1aa50de --- /dev/null +++ b/.clang-format @@ -0,0 +1,21 @@ +# This is the standard clang format file for CPP files for +# the j7s project. +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -4 +AlignAfterOpenBracket: AlwaysBreak +BreakBeforeBraces: Allman +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +AllowShortBlocksOnASingleLine: false +BreakConstructorInitializers: AfterColon +BreakInheritanceList: AfterColon +AllowShortFunctionsOnASingleLine: Inline +IndentWidth: 4 +... diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..35fc4d4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 4.2) +project(j7s_zenoh_bridge) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(fmt REQUIRED) + +find_package(zenohc) +find_package(zenohcxx) + +add_library(zenoh_bridge_component SHARED src/ZenohBridge.cpp) +target_include_directories(zenoh_bridge_component PUBLIC + $ + $) +target_link_libraries(zenoh_bridge_component + rclcpp::rclcpp + rclcpp_components::component + zenohcxx::zenohc + fmt::fmt +) + +rclcpp_components_register_node(zenoh_bridge_component + PLUGIN "j7s::ZenohBridge" + EXECUTABLE zenoh_bridge +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +install(TARGETS zenoh_bridge_component + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install launch and param files. +install(DIRECTORY + launch params + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/include/j7s_zenoh_bridge/ZenohBridge.hpp b/include/j7s_zenoh_bridge/ZenohBridge.hpp new file mode 100644 index 0000000..dfcb7f1 --- /dev/null +++ b/include/j7s_zenoh_bridge/ZenohBridge.hpp @@ -0,0 +1,64 @@ +// +// Copyright 2026 James Pace +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. +// +// This Source Code Form is "Incompatible With Secondary Licenses", as +// defined by the Mozilla Public License, v. 2.0. +// +#pragma once +#include + +#include "zenoh.hxx" + +namespace j7s +{ + +class Config +{ +public: + Config(rclcpp::Node & node, const std::string & prefix); + + // TODO: Should we specify QoS here? + std::string m_rosTopic; + std::string m_zenohKeyExpr; + std::string m_rosType; +}; + +class ZenohSource +{ +public: + ZenohSource( + rclcpp::Node & node, const std::shared_ptr session, const Config & config); + +private: + std::shared_ptr m_publisher; + std::shared_ptr> m_subscriber; +}; + +class RosSource +{ +public: + RosSource( + rclcpp::Node & node, const std::shared_ptr session, const Config & config); + +private: + std::shared_ptr m_publisher; + std::shared_ptr m_subscriber; +}; + +class ZenohBridge : public rclcpp::Node +{ +public: + ZenohBridge(const rclcpp::NodeOptions & options); + +private: + std::vector m_zenohSources; + std::vector m_rosSources; + + std::shared_ptr m_zenohSession; +}; + +} // namespace j7s diff --git a/launch/test.launch.xml b/launch/test.launch.xml new file mode 100644 index 0000000..90c0e7c --- /dev/null +++ b/launch/test.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..56ef47d --- /dev/null +++ b/package.xml @@ -0,0 +1,17 @@ + + + + j7s_zenoh_bridge + 0.0.0 + TODO: Package description + jimmy + MPL 2.0 + + ament_cmake + + rclcpp_components + + + ament_cmake + + diff --git a/params/in.yaml b/params/in.yaml new file mode 100644 index 0000000..efaebf3 --- /dev/null +++ b/params/in.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + ros_sources: + - a_source + + a_source: + ros_topic: "/ros/test_in" + zenoh_key_expr: "ros/test" + ros_type: "std_msgs/msg/String" \ No newline at end of file diff --git a/params/out.yaml b/params/out.yaml new file mode 100644 index 0000000..1dc6ca6 --- /dev/null +++ b/params/out.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + zenoh_sources: + - a_source + + a_source: + ros_topic: "/ros/test_out" + zenoh_key_expr: "ros/test" + ros_type: "std_msgs/msg/String" \ No newline at end of file diff --git a/src/ZenohBridge.cpp b/src/ZenohBridge.cpp new file mode 100644 index 0000000..f3abe71 --- /dev/null +++ b/src/ZenohBridge.cpp @@ -0,0 +1,104 @@ +// +// Copyright 2026 James Pace +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. +// +// This Source Code Form is "Incompatible With Secondary Licenses", as +// defined by the Mozilla Public License, v. 2.0. +// +#include + +#include +#include + +namespace j7s +{ + +ZenohBridge::ZenohBridge(const rclcpp::NodeOptions & options) : Node("zenoh_bridge", options) +{ + // Should this be from a file? + // TODO: static inline Config from_file(const std::string &path, ZResult *err = nullptr) + auto config = zenoh::Config::create_default(); + m_zenohSession = std::make_shared(std::move(config)); + + const auto emptyVector = std::vector(); + const auto zenohSourceNames = + this->declare_parameter>("zenoh_sources", emptyVector); + const auto rosSourceNames = + this->declare_parameter>("ros_sources", emptyVector); + + // Make all the zenoh subs. + for (const auto & source : zenohSourceNames) + { + const Config config(*this, source); + m_zenohSources.emplace_back(*this, m_zenohSession, config); + } + + // Make all the ros subs. + for (const auto & source : rosSourceNames) + { + const Config config(*this, source); + m_rosSources.emplace_back(*this, m_zenohSession, config); + } +} + +Config::Config(rclcpp::Node & node, const std::string & prefix) +{ + m_rosTopic = node.declare_parameter(fmt::format("{}.ros_topic", prefix)); + m_zenohKeyExpr = node.declare_parameter(fmt::format("{}.zenoh_key_expr", prefix)); + m_rosType = node.declare_parameter(fmt::format("{}.ros_type", prefix)); +} + +ZenohSource::ZenohSource( + rclcpp::Node & node, const std::shared_ptr session, const Config & config) +{ + m_publisher = node.create_generic_publisher(config.m_rosTopic, config.m_rosType, 1); + + m_subscriber = std::make_shared>(session->declare_subscriber( + zenoh::KeyExpr(config.m_zenohKeyExpr), + [this](const zenoh::Sample & sample) + { + const std::vector message = sample.get_payload().as_vector(); + + rcutils_uint8_array_t toPublish; + const auto defaltAllocator = rcutils_get_default_allocator(); + rcutils_ret_t retVal; + retVal = rcutils_uint8_array_init(&toPublish, message.size(), &defaltAllocator); + if (retVal != RCUTILS_RET_OK) + { + throw std::runtime_error("Failed to allocate memory."); + } + std::copy(message.begin(), message.end(), toPublish.buffer); + toPublish.buffer_length = message.size(); + + m_publisher->publish(rclcpp::SerializedMessage(toPublish)); + }, + zenoh::closures::none)); +} + +RosSource::RosSource( + rclcpp::Node & node, const std::shared_ptr session, const Config & config) +{ + m_publisher = std::make_shared( + session->declare_publisher(zenoh::KeyExpr(config.m_zenohKeyExpr))); + + // TODO: Allow QoS to be set. + m_subscriber = node.create_generic_subscription( + config.m_rosTopic, config.m_rosType, 1, + [this](const rclcpp::SerializedMessage & msg) + { + rcutils_uint8_array_t rawData = msg.get_rcl_serialized_message(); + const std::vector dataAsVector( + rawData.buffer, rawData.buffer + rawData.buffer_length); + zenoh::Bytes dataForZenoh(std::move(dataAsVector)); + + m_publisher->put(std::move(dataForZenoh)); + }); +} + +}; // namespace j7s + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(j7s::ZenohBridge)