Init commit.

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 <fmt/core.h>
+
+#include <algorithm>
+#include <j7s_zenoh_bridge/ZenohBridge.hpp>
+
+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<zenoh::Session>(std::move(config));
+
+    const auto emptyVector = std::vector<std::string>();
+    const auto zenohSourceNames =
+        this->declare_parameter<std::vector<std::string>>("zenoh_sources", emptyVector);
+    const auto rosSourceNames =
+        this->declare_parameter<std::vector<std::string>>("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<std::string>(fmt::format("{}.ros_topic", prefix));
+    m_zenohKeyExpr = node.declare_parameter<std::string>(fmt::format("{}.zenoh_key_expr", prefix));
+    m_rosType = node.declare_parameter<std::string>(fmt::format("{}.ros_type", prefix));
+}
+
+ZenohSource::ZenohSource(
+    rclcpp::Node & node, const std::shared_ptr<zenoh::Session> session, const Config & config)
+{
+    m_publisher = node.create_generic_publisher(config.m_rosTopic, config.m_rosType, 1);
+
+    m_subscriber = std::make_shared<zenoh::Subscriber<void>>(session->declare_subscriber(
+        zenoh::KeyExpr(config.m_zenohKeyExpr),
+        [this](const zenoh::Sample & sample)
+        {
+            const std::vector<uint8_t> 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<zenoh::Session> session, const Config & config)
+{
+    m_publisher = std::make_shared<zenoh::Publisher>(
+        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<uint8_t> 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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(j7s::ZenohBridge)