File ros2_bridge.hpp

File List > astutedds > dcps > ros2_bridge.hpp

Go to the documentation of this file

//
// Copyright (c) 2026, Astute Systems PTY LTD
//
// This file is part of the Astute DDS developed by Astute Systems.
//
// See the commercial LICENSE file in the project root for full license details.
//
// @file ros2_bridge.hpp
// @brief Lightweight ROS 2 to DDS bridge helpers
//
// Provides ROS 2 topic/type name mapping helpers and a serialized payload bridge
// built on top of the existing DCPS API.
//

#ifndef ASTUTEDDS_DCPS_ROS2_BRIDGE_HPP
#define ASTUTEDDS_DCPS_ROS2_BRIDGE_HPP

#include <astutedds/dcps/dcps.hpp>

#include <functional>
#include <string>
#include <string_view>
#include <unordered_map>
#include <utility>
#include <vector>

namespace astutedds::dcps
{

inline std::string normalize_ros2_topic_name(std::string_view topic_name)
{
    if (topic_name.empty())
    {
        return {};
    }

    const auto first = topic_name.find_first_not_of('/');
    if (first == std::string_view::npos)
    {
        return {};
    }

    const auto last = topic_name.find_last_not_of('/');
    return std::string(topic_name.substr(first, last - first + 1));
}

inline bool is_ros2_dds_topic_name(std::string_view topic_name)
{
    return topic_name.rfind("rt/", 0) == 0 || topic_name.rfind("rq/", 0) == 0 ||
           topic_name.rfind("rr/", 0) == 0 || topic_name.rfind("ra/", 0) == 0 ||
           topic_name.rfind("rp/", 0) == 0;
}

inline std::string ros2_topic_to_dds_topic(std::string_view topic_name)
{
    const std::string normalized = normalize_ros2_topic_name(topic_name);
    if (normalized.empty())
    {
        return {};
    }

    if (is_ros2_dds_topic_name(normalized))
    {
        return normalized;
    }

    return "rt/" + normalized;
}

inline std::string ros2_type_to_dds_type(std::string_view type_name)
{
    if (type_name.empty())
    {
        return {};
    }

    const std::string type_string(type_name);
    if (type_string.find("::dds_::") != std::string::npos)
    {
        return type_string;
    }

    std::vector<std::string> parts;
    size_t start = 0;
    while (start <= type_string.size())
    {
        const size_t separator = type_string.find('/', start);
        const std::string part = type_string.substr(start, separator - start);
        if (part.empty())
        {
            return {};
        }

        parts.push_back(part);
        if (separator == std::string::npos)
        {
            break;
        }
        start = separator + 1;
    }

    if (parts.size() < 2)
    {
        return {};
    }

    std::string dds_type_name;
    for (size_t i = 0; i + 1 < parts.size(); ++i)
    {
        if (!dds_type_name.empty())
        {
            dds_type_name += "::";
        }
        dds_type_name += parts[i];
    }

    dds_type_name += "::dds_::";
    dds_type_name += parts.back();
    if (!dds_type_name.empty() && dds_type_name.back() != '_')
    {
        dds_type_name.push_back('_');
    }

    return dds_type_name;
}

class ROS2Bridge
{
public:
    using SubscriptionCallback = DataReader::DataCallback;

    explicit ROS2Bridge(DomainParticipant* participant) : participant_(participant) {}

    bool register_topic(const std::string& ros2_topic_name,
                        const std::string& ros2_type_name,
                        const TopicQos& topic_qos = TopicQos{},
                        const DataWriterQos& writer_qos = DataWriterQos{},
                        const DataReaderQos& reader_qos = DataReaderQos{})
    {
        if (!participant_)
        {
            return false;
        }

        const std::string dds_topic_name = ros2_topic_to_dds_topic(ros2_topic_name);
        const std::string topic_key = normalize_ros2_topic_name(dds_topic_name);
        const std::string dds_type_name = ros2_type_to_dds_type(ros2_type_name);
        if (topic_key.empty() || dds_topic_name.empty() || dds_type_name.empty())
        {
            return false;
        }

        const auto existing = bindings_.find(topic_key);
        if (existing != bindings_.end())
        {
            if (existing->second.dds_type_name != dds_type_name)
            {
                return false;
            }

            const Topic* const existing_topic = existing->second.topic;
            if (!existing_topic)
            {
                return false;
            }

            const bool topic_qos_matches = existing_topic->get_qos() == topic_qos;
            const bool writer_qos_matches = existing->second.writer_qos == writer_qos;
            const bool reader_qos_matches = existing->second.reader_qos == reader_qos;
            return topic_qos_matches && writer_qos_matches && reader_qos_matches;
        }

        Topic* topic = participant_->create_topic(dds_topic_name, dds_type_name, topic_qos);
        if (!topic)
        {
            return false;
        }

        TopicBinding binding;
        binding.topic = topic;
        binding.dds_topic_name = dds_topic_name;
        binding.dds_type_name = dds_type_name;
        binding.writer_qos = writer_qos;
        binding.reader_qos = reader_qos;
        bindings_.emplace(topic_key, std::move(binding));
        return true;
    }

    bool publish_serialized(const std::string& ros2_topic_name,
                            const std::vector<uint8_t>& serialized_message)
    {
        TopicBinding* binding = find_binding(ros2_topic_name);
        if (!binding)
        {
            return false;
        }

        DataWriter* writer = ensure_writer(*binding);
        return writer ? writer->write(serialized_message) : false;
    }

    bool subscribe_serialized(const std::string& ros2_topic_name, SubscriptionCallback callback)
    {
        TopicBinding* binding = find_binding(ros2_topic_name);
        if (!binding || !callback)
        {
            return false;
        }

        DataReader* reader = ensure_reader(*binding);
        if (!reader)
        {
            return false;
        }

        reader->set_data_callback(std::move(callback));
        return true;
    }

    ReturnCode_t read_next_serialized(const std::string& ros2_topic_name,
                                      std::vector<uint8_t>& data,
                                      SampleInfo& info)
    {
        TopicBinding* binding = find_binding(ros2_topic_name);
        if (!binding)
        {
            return ReturnCode_t::RETCODE_BAD_PARAMETER;
        }

        DataReader* reader = ensure_reader(*binding);
        return reader ? reader->read_next_sample(data, info) : ReturnCode_t::RETCODE_ERROR;
    }

    ReturnCode_t take_next_serialized(const std::string& ros2_topic_name,
                                      std::vector<uint8_t>& data,
                                      SampleInfo& info)
    {
        TopicBinding* binding = find_binding(ros2_topic_name);
        if (!binding)
        {
            return ReturnCode_t::RETCODE_BAD_PARAMETER;
        }

        DataReader* reader = ensure_reader(*binding);
        return reader ? reader->take_next_sample(data, info) : ReturnCode_t::RETCODE_ERROR;
    }

    Topic* topic(const std::string& ros2_topic_name) const
    {
        const TopicBinding* binding = find_binding(ros2_topic_name);
        return binding ? binding->topic : nullptr;
    }

    DataWriter* writer(const std::string& ros2_topic_name) const
    {
        const TopicBinding* binding = find_binding(ros2_topic_name);
        return binding ? binding->writer : nullptr;
    }

    DataReader* reader(const std::string& ros2_topic_name) const
    {
        const TopicBinding* binding = find_binding(ros2_topic_name);
        return binding ? binding->reader : nullptr;
    }

    std::string dds_topic_name(const std::string& ros2_topic_name) const
    {
        const TopicBinding* binding = find_binding(ros2_topic_name);
        return binding ? binding->dds_topic_name : std::string{};
    }

    std::string dds_type_name(const std::string& ros2_topic_name) const
    {
        const TopicBinding* binding = find_binding(ros2_topic_name);
        return binding ? binding->dds_type_name : std::string{};
    }

private:
    struct TopicBinding
    {
        std::string dds_topic_name;
        std::string dds_type_name;
        Topic* topic{nullptr};
        DataWriter* writer{nullptr};
        DataReader* reader{nullptr};
        DataWriterQos writer_qos{};
        DataReaderQos reader_qos{};
    };

    TopicBinding* find_binding(const std::string& ros2_topic_name)
    {
        const std::string key = normalize_ros2_topic_name(ros2_topic_to_dds_topic(ros2_topic_name));
        const auto it = bindings_.find(key);
        return it != bindings_.end() ? &it->second : nullptr;
    }

    const TopicBinding* find_binding(const std::string& ros2_topic_name) const
    {
        const std::string key = normalize_ros2_topic_name(ros2_topic_to_dds_topic(ros2_topic_name));
        const auto it = bindings_.find(key);
        return it != bindings_.end() ? &it->second : nullptr;
    }

    Publisher* ensure_publisher()
    {
        if (!participant_)
        {
            return nullptr;
        }

        if (!publisher_)
        {
            publisher_ = participant_->create_publisher();
        }
        return publisher_;
    }

    Subscriber* ensure_subscriber()
    {
        if (!participant_)
        {
            return nullptr;
        }

        if (!subscriber_)
        {
            subscriber_ = participant_->create_subscriber();
        }
        return subscriber_;
    }

    DataWriter* ensure_writer(TopicBinding& binding)
    {
        if (!binding.writer)
        {
            Publisher* publisher = ensure_publisher();
            if (!publisher)
            {
                return nullptr;
            }

            binding.writer = publisher->create_datawriter(binding.topic, binding.writer_qos);
        }
        return binding.writer;
    }

    DataReader* ensure_reader(TopicBinding& binding)
    {
        if (!binding.reader)
        {
            Subscriber* subscriber = ensure_subscriber();
            if (!subscriber)
            {
                return nullptr;
            }

            binding.reader = subscriber->create_datareader(binding.topic, binding.reader_qos);
        }
        return binding.reader;
    }

    DomainParticipant* participant_{nullptr};
    Publisher* publisher_{nullptr};
    Subscriber* subscriber_{nullptr};
    std::unordered_map<std::string, TopicBinding> bindings_;
};

}  // namespace astutedds::dcps

#endif  // ASTUTEDDS_DCPS_ROS2_BRIDGE_HPP