Init commit.
This commit is contained in:
commit
c38407bbd8
|
|
@ -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
|
||||||
|
...
|
||||||
|
|
@ -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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
|
||||||
|
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()
|
||||||
|
|
@ -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 <rclcpp/rclcpp.hpp>
|
||||||
|
|
||||||
|
#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<zenoh::Session> session, const Config & config);
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<rclcpp::GenericPublisher> m_publisher;
|
||||||
|
std::shared_ptr<zenoh::Subscriber<void>> m_subscriber;
|
||||||
|
};
|
||||||
|
|
||||||
|
class RosSource
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
RosSource(
|
||||||
|
rclcpp::Node & node, const std::shared_ptr<zenoh::Session> session, const Config & config);
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<zenoh::Publisher> m_publisher;
|
||||||
|
std::shared_ptr<rclcpp::GenericSubscription> m_subscriber;
|
||||||
|
};
|
||||||
|
|
||||||
|
class ZenohBridge : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ZenohBridge(const rclcpp::NodeOptions & options);
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::vector<ZenohSource> m_zenohSources;
|
||||||
|
std::vector<RosSource> m_rosSources;
|
||||||
|
|
||||||
|
std::shared_ptr<zenoh::Session> m_zenohSession;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace j7s
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
<launch>
|
||||||
|
<node name="in_node" pkg="j7s_zenoh_bridge" exec="zenoh_bridge">
|
||||||
|
<param from="$(find-pkg-share j7s_zenoh_bridge)/params/in.yaml"/>
|
||||||
|
</node>
|
||||||
|
<node name="out_node" pkg="j7s_zenoh_bridge" exec="zenoh_bridge">
|
||||||
|
<param from="$(find-pkg-share j7s_zenoh_bridge)/params/out.yaml"/>
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,17 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>j7s_zenoh_bridge</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="jpace121@gmail.com">jimmy</maintainer>
|
||||||
|
<license>MPL 2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp_components</depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -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"
|
||||||
|
|
@ -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"
|
||||||
|
|
@ -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)
|
||||||
Loading…
Reference in New Issue