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