ReplierImpl.cpp

// Copyright 2025 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "ReplierImpl.hpp"

#include <string>

#include <fastdds/dds/builtin/topic/PublicationBuiltinTopicData.hpp>
#include <fastdds/dds/builtin/topic/SubscriptionBuiltinTopicData.hpp>
#include <fastdds/dds/core/condition/Condition.hpp>
#include <fastdds/dds/core/detail/DDSReturnCode.hpp>
#include <fastdds/dds/core/LoanableCollection.hpp>
#include <fastdds/dds/core/LoanableSequence.hpp>
#include <fastdds/dds/core/status/PublicationMatchedStatus.hpp>
#include <fastdds/dds/core/status/StatusMask.hpp>
#include <fastdds/dds/core/status/SubscriptionMatchedStatus.hpp>
#include <fastdds/dds/domain/qos/ReplierQos.hpp>
#include <fastdds/dds/log/Log.hpp>
#include <fastdds/dds/rpc/RequestInfo.hpp>
#include <fastdds/rtps/common/SampleIdentity.hpp>
#include <fastdds/rtps/common/WriteParams.hpp>

#include "ServiceImpl.hpp"

namespace eprosima {
namespace fastdds {
namespace dds {
namespace rpc {

/**
 * @brief Fills the related sample identity of the request.
 *
 * This will fill the related sample identity of the request with values taken from the sample identity.
 * Values different from unknown are preserved.
 *
 * @param info [in,out] The request information to update.
 */
static void fill_related_sample_identity(
        RequestInfo& info)
{
    // When sending a reply, the code here expects that related_sample_identity
    // has the sample_identity of the corresponding request.

    static const rtps::SampleIdentity unknown_identity = rtps::SampleIdentity::unknown();

    // If the related guid is unknown, we consider that the request is not related to a previous one,
    // so we set the related sample identity to the received sample identity
    if (unknown_identity.writer_guid() == info.related_sample_identity.writer_guid())
    {
        info.related_sample_identity = info.sample_identity;
        return;
    }

    // There is a special case where only the related guid is set.
    // This is used in ROS 2 to convey the GUID of the reply reader.
    // In this case we just set the sequence number of the related sample identity
    if (unknown_identity.sequence_number() == info.related_sample_identity.sequence_number())
    {
        info.related_sample_identity.sequence_number() = info.sample_identity.sequence_number();
    }
}

ReplierImpl::ReplierImpl(
        ServiceImpl* service,
        const ReplierQos& qos)
    : replier_reader_(nullptr)
    , replier_writer_(nullptr)
    , qos_(qos)
    , service_(service)
    , enabled_(false)
    , matched_status_changed_(false)
{
}

ReplierImpl::~ReplierImpl()
{
    close();
    service_ = nullptr;
}

const std::string& ReplierImpl::get_service_name() const
{
    return service_->get_service_name();
}

ReturnCode_t ReplierImpl::send_reply(
        void* data,
        const RequestInfo& info)
{
    if (!enabled_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Trying to send a reply with a disabled replier");
        return RETCODE_PRECONDITION_NOT_MET;
    }

    Time_t timeout{3, 0}; // Default timeout of 3 seconds
    auto match_status = wait_for_matching(timeout, info);
    if (RequesterMatchStatus::UNMATCHED == match_status)
    {
        // The writer that sent the request has been unmatched.
        EPROSIMA_LOG_WARNING(REPLIER, "Trying to send a reply to a disconnected requester");
        return RETCODE_NO_DATA;
    }
    else if (RequesterMatchStatus::PARTIALLY_MATCHED == match_status)
    {
        // The writer that sent the request is still matched, but the reply topic is not.
        EPROSIMA_LOG_WARNING(REPLIER, "Trying to send a reply to a partially matched requester");
        return RETCODE_TIMEOUT;
    }

    rtps::WriteParams wparams;
    wparams.related_sample_identity(info.related_sample_identity);
    wparams.has_more_replies(info.has_more_replies);

    return replier_writer_->write(data, wparams);
}

ReturnCode_t ReplierImpl::take_request(
        void* data,
        RequestInfo& info)
{
    ReturnCode_t retcode;

    if (!enabled_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Trying to take a request with a disabled replier");
        return RETCODE_PRECONDITION_NOT_MET;
    }

    retcode = replier_reader_->take_next_sample(data, &info);
    fill_related_sample_identity(info);

    return retcode;
}

ReturnCode_t ReplierImpl::take_request(
        LoanableCollection& data,
        LoanableSequence<RequestInfo>& info)
{
    ReturnCode_t retcode;

    if (!enabled_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Trying to take a request with a disabled replier");
        return RETCODE_PRECONDITION_NOT_MET;
    }

    retcode = replier_reader_->take(data, info);

    // Fill related_sample_identity attribute
    for (LoanableCollection::size_type i = 0; i < info.length(); ++i)
    {
        fill_related_sample_identity(info[i]);
    }

    return retcode;
}

ReturnCode_t ReplierImpl::return_loan(
        LoanableCollection& data,
        LoanableSequence<RequestInfo>& info)
{
    if (!enabled_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Trying to return loan with a disabled replier");
        return RETCODE_PRECONDITION_NOT_MET;
    }

    return replier_reader_->return_loan(data, info);
}

ReturnCode_t ReplierImpl::enable()
{
    ReturnCode_t retcode = RETCODE_OK;

    if (!enabled_)
    {
        if (!service_)
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Service is nullptr");
            return RETCODE_ERROR;
        }

        if (!service_->is_enabled())
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Trying to enable Replier on a disabled Service");
            return RETCODE_PRECONDITION_NOT_MET;
        }

        retcode = create_dds_entities(qos_);

        if (RETCODE_OK != retcode)
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Unable to enable replier");
            // If any error occurs, delete the created entities
            // This is necessary to avoid keeping requester in an inconsistent state
            delete_contained_entities();
            return retcode;
        }
        else
        {
            replier_reader_->enable();
            replier_writer_->enable();
        }

        enabled_ = true;
    }

    return retcode;
}

ReturnCode_t ReplierImpl::close()
{
    ReturnCode_t retcode = RETCODE_OK;

    if (enabled_)
    {
        retcode = delete_contained_entities();

        if (RETCODE_OK != retcode)
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Error deleting DDS entities");
            return retcode;
        }

        enabled_ = false;
    }

    return retcode;
}

void ReplierImpl::on_publication_matched(
        DataWriter* /*writer*/,
        const PublicationMatchedStatus& /*info*/)
{

    matched_status_changed_.store(true);
    cv_.notify_one();
}

void ReplierImpl::on_subscription_matched(
        DataReader* /*reader*/,
        const SubscriptionMatchedStatus& /*info*/)
{
    matched_status_changed_.store(true);
    cv_.notify_one();
}

ReturnCode_t ReplierImpl::create_dds_entities(
        const ReplierQos& qos)
{
    // Entities are not autoenabled since the publisher and
    // the subscriber have autoenable_created_entities set to false.

    // Create writer for the Reply topic
    replier_writer_ =
            service_->get_publisher()->create_datawriter(
        service_->get_reply_topic(), qos.writer_qos, this, StatusMask::publication_matched());

    if (!replier_writer_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Error creating replier writer");
        return RETCODE_ERROR;
    }

    replier_reader_ =
            service_->get_subscriber()->create_datareader(
        service_->get_request_topic(), qos.reader_qos, this, StatusMask::subscription_matched());

    if (!replier_reader_)
    {
        EPROSIMA_LOG_ERROR(REPLIER, "Error creating reply reader");
        return RETCODE_ERROR;
    }

    // Set the related entity key on both entities
    replier_reader_->set_related_datawriter(replier_writer_);
    replier_writer_->set_related_datareader(replier_reader_);

    return RETCODE_OK;
}

ReturnCode_t ReplierImpl::delete_contained_entities()
{
    // Check if DataWriter and DataReader can be deleted.
    // If not, do nothing and return an error code
    if (replier_writer_)
    {
        if (!service_->get_publisher()->can_be_deleted(replier_writer_))
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Replier DataWriter cannot be deleted");
            return RETCODE_PRECONDITION_NOT_MET;
        }
    }

    if (replier_reader_)
    {
        if (!service_->get_subscriber()->can_be_deleted(replier_reader_))
        {
            EPROSIMA_LOG_ERROR(REPLIER, "Replier DataReader cannot be deleted");
            return RETCODE_PRECONDITION_NOT_MET;
        }
    }

    // Delete DataWriter and DataReader
    service_->get_publisher()->delete_datawriter(replier_writer_);
    replier_writer_ = nullptr;
    service_->get_subscriber()->delete_datareader(replier_reader_);
    replier_reader_ = nullptr;

    return RETCODE_OK;
}

ReplierImpl::RequesterMatchStatus ReplierImpl::requester_match_status(
        const RequestInfo& info) const
{
    // Check if the replier is still matched with the requester in the request topic
    PublicationBuiltinTopicData pub_data;
    if (RETCODE_OK != replier_reader_->get_matched_publication_data(pub_data, info.sample_identity.writer_guid()))
    {
        return RequesterMatchStatus::UNMATCHED;
    }

    auto related_guid = info.related_sample_identity.writer_guid();
    bool reply_topic_matched = false;
    if (related_guid.entityId.is_reader() && info.sample_identity.writer_guid() != related_guid)
    {
        // Custom related GUID (i.e. reply reader GUID) sent with the request.
        // Check if the replier writer is matched with that specific reader.
        SubscriptionBuiltinTopicData sub_data;
        reply_topic_matched = RETCODE_OK == replier_writer_->get_matched_subscription_data(sub_data, related_guid);
    }
    else
    {
        // Take the replier reader GUID from the related_datareader_key within PublicationBuiltinTopicData
        SubscriptionBuiltinTopicData related_reader_sub_data;
        reply_topic_matched = RETCODE_OK == replier_writer_->get_matched_subscription_data(related_reader_sub_data,
                        pub_data.related_datareader_key);
    }

    return reply_topic_matched ?
           RequesterMatchStatus::MATCHED :
           RequesterMatchStatus::PARTIALLY_MATCHED;
}

ReplierImpl::RequesterMatchStatus ReplierImpl::wait_for_matching(
        const fastdds::dds::Duration_t& timeout,
        const RequestInfo& info)
{
    Time_t current_time;
    Time_t::now(current_time);
    Time_t finish_time = current_time + timeout;

    RequesterMatchStatus requester_status = requester_match_status(info);
    while ((RequesterMatchStatus::MATCHED != requester_status) && current_time < finish_time)
    {
        // Wait for the matched status to change
        // Or every 100 milliseconds.
        std::unique_lock<std::mutex> lock(mtx_);
        bool res = cv_.wait_for(lock,
                        std::chrono::milliseconds(100),
                        [this]()
                        {
                            return matched_status_changed_.load();
                        });

        if (res)
        {
            // Reset the matched status changed flag
            matched_status_changed_.store(false);

            // Check if the requester is fully matched
            requester_status = requester_match_status(info);
        }

        // Update the current time
        Time_t::now(current_time);
    }

    return requester_status;
}

} // namespace rpc
} // namespace dds
} // namespace fastdds
} // namespace eprosima