custom_publisher_info.cpp

// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved.
//
// 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 "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"

#include "fastdds/dds/core/policy/QosPolicies.hpp"
#include "fastdds/dds/core/status/BaseStatus.hpp"
#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp"

#include "event_helpers.hpp"
#include "types/event_types.hpp"

#include "rcpputils/unique_lock.hpp"

EventListenerInterface *
CustomPublisherInfo::get_listener() const
{
  return publisher_event_;
}

CustomDataWriterListener::CustomDataWriterListener(RMWPublisherEvent * pub_event)
: publisher_event_(pub_event)
{
}

void CustomDataWriterListener::on_publication_matched(
  eprosima::fastdds::dds::DataWriter * writer,
  const eprosima::fastdds::dds::PublicationMatchedStatus & status)
{
  (void)writer;

  if (status.current_count_change == 1) {
    publisher_event_->track_unique_subscription(
      eprosima::fastdds::rtps::iHandle2GUID(status.last_subscription_handle));
  } else if (status.current_count_change == -1) {
    publisher_event_->untrack_unique_subscription(
      eprosima::fastdds::rtps::iHandle2GUID(status.last_subscription_handle));
  } else {
    return;
  }

  publisher_event_->update_matched(
    status.total_count,
    status.total_count_change,
    status.current_count,
    status.current_count_change);
}


void
CustomDataWriterListener::on_offered_deadline_missed(
  eprosima::fastdds::dds::DataWriter * writer,
  const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status)
{
  (void)writer;

  publisher_event_->update_deadline(status.total_count, status.total_count_change);
}

void CustomDataWriterListener::on_liveliness_lost(
  eprosima::fastdds::dds::DataWriter * writer,
  const eprosima::fastdds::dds::LivelinessLostStatus & status)
{
  (void)writer;

  publisher_event_->update_liveliness_lost(status.total_count, status.total_count_change);
}

void CustomDataWriterListener::on_offered_incompatible_qos(
  eprosima::fastdds::dds::DataWriter * writer,
  const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status)
{
  (void)writer;

  publisher_event_->update_offered_incompatible_qos(
    status.last_policy_id, status.total_count,
    status.total_count_change);
}

RMWPublisherEvent::RMWPublisherEvent(CustomPublisherInfo * info)
: publisher_info_(info),
  deadline_changed_(false),
  liveliness_changed_(false),
  incompatible_qos_changed_(false),
  matched_changes_(false)
{
}

eprosima::fastdds::dds::StatusCondition & RMWPublisherEvent::get_statuscondition() const
{
  return publisher_info_->data_writer_->get_statuscondition();
}

bool RMWPublisherEvent::take_event(
  rmw_event_type_t event_type,
  void * event_info)
{
  assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type));

  // NOTE: the DataWriter status queries below (get_*_status) acquire the writer's
  // internal mutex. Fast-DDS may invoke this publisher's listener callbacks -- which
  // take on_new_event_m_ -- while holding that writer mutex. To avoid a lock-order
  // inversion (ABBA deadlock) with the writer's threads, on_new_event_m_ must NOT be
  // held across a writer get_*_status() call. Each case that queries the writer releases
  // the lock around the query, copies into a local, then re-acquires and adopts the
  // value only if a listener update has not arrived in the meantime. (Symmetric to the
  // subscription-side fix in custom_subscriber_info.cpp.)
  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  switch (event_type) {
    case RMW_EVENT_LIVELINESS_LOST:
      {
        auto rmw_data = static_cast<rmw_liveliness_lost_status_t *>(event_info);
        if (liveliness_changed_) {
          liveliness_changed_ = false;
        } else {
          eprosima::fastdds::dds::LivelinessLostStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_liveliness_lost_status(current);
          lock_mutex.lock();
          if (!liveliness_changed_) {
            liveliness_lost_status_ = current;
          }
          liveliness_changed_ = false;
        }
        rmw_data->total_count = liveliness_lost_status_.total_count;
        rmw_data->total_count_change = liveliness_lost_status_.total_count_change;
        liveliness_lost_status_.total_count_change = 0;
      }
      break;
    case RMW_EVENT_OFFERED_DEADLINE_MISSED:
      {
        auto rmw_data = static_cast<rmw_offered_deadline_missed_status_t *>(event_info);
        if (deadline_changed_) {
          deadline_changed_ = false;
        } else {
          eprosima::fastdds::dds::OfferedDeadlineMissedStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_offered_deadline_missed_status(current);
          lock_mutex.lock();
          if (!deadline_changed_) {
            offered_deadline_missed_status_ = current;
          }
          deadline_changed_ = false;
        }
        rmw_data->total_count = offered_deadline_missed_status_.total_count;
        rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change;
        offered_deadline_missed_status_.total_count_change = 0;
      }
      break;
    case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE:
      {
        auto rmw_data = static_cast<rmw_offered_qos_incompatible_event_status_t *>(event_info);
        if (incompatible_qos_changed_) {
          incompatible_qos_changed_ = false;
        } else {
          eprosima::fastdds::dds::OfferedIncompatibleQosStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_offered_incompatible_qos_status(current);
          lock_mutex.lock();
          if (!incompatible_qos_changed_) {
            incompatible_qos_status_ = current;
          }
          incompatible_qos_changed_ = false;
        }
        rmw_data->total_count = incompatible_qos_status_.total_count;
        rmw_data->total_count_change = incompatible_qos_status_.total_count_change;
        rmw_data->last_policy_kind =
          rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy(
          incompatible_qos_status_.last_policy_id);
        incompatible_qos_status_.total_count_change = 0;
      }
      break;
    case RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE:
      {
        // get_inconsistent_topic_status() operates on the Topic, not the DataWriter, so
        // it does not take the writer mutex and is not part of the writer-mutex inversion;
        // left querying under the lock unchanged.
        auto rmw_data = static_cast<rmw_incompatible_type_status_t *>(event_info);
        if (inconsistent_topic_changed_) {
          inconsistent_topic_changed_ = false;
        } else {
          publisher_info_->data_writer_->get_topic()->get_inconsistent_topic_status(
            inconsistent_topic_status_);
        }
        rmw_data->total_count = inconsistent_topic_status_.total_count;
        rmw_data->total_count_change = inconsistent_topic_status_.total_count_change;
        inconsistent_topic_status_.total_count_change = 0;
      }
      break;
    case RMW_EVENT_PUBLICATION_MATCHED:
      {
        auto rmw_data = static_cast<rmw_matched_status_t *>(event_info);

        eprosima::fastdds::dds::PublicationMatchedStatus matched_status;
        lock_mutex.unlock();
        publisher_info_->data_writer_->get_publication_matched_status(matched_status);
        lock_mutex.lock();

        rmw_data->total_count = static_cast<size_t>(matched_status.total_count);
        rmw_data->current_count = static_cast<size_t>(matched_status.current_count);
        rmw_data->total_count_change = static_cast<size_t>(matched_status.total_count_change);
        rmw_data->current_count_change = matched_status.current_count_change;

        if (matched_changes_) {
          rmw_data->total_count_change += static_cast<size_t>(matched_status_.total_count_change);
          rmw_data->current_count_change += matched_status_.current_count_change;
          matched_changes_ = false;
        }

        matched_status_.total_count_change = 0;
        matched_status_.current_count_change = 0;
      }
      break;
    default:
      return false;
  }
  event_guard[event_type].set_trigger_value(false);

  return true;
}

void RMWPublisherEvent::set_on_new_event_callback(
  rmw_event_type_t event_type,
  const void * user_data,
  rmw_event_callback_t callback)
{
  // NOTE: like take_event(), this method must not hold on_new_event_m_ across any
  // DataWriter call (get_status_mask / get_*_status / set_listener): those acquire the
  // writer's internal mutex, and Fast-DDS may invoke this publisher's listener callbacks
  // -- which take on_new_event_m_ -- while holding that writer mutex. Holding
  // on_new_event_m_ across a writer call is therefore a lock-order inversion (ABBA
  // deadlock). The lock is released around every writer query below; each get_*_status
  // copies into a local and re-acquires before adopting, keeping any value a listener
  // delivered during the unlocked window. get_inconsistent_topic_status() operates on
  // the Topic (not the writer) and is left under the lock, as in take_event().
  eprosima::fastdds::dds::StatusMask status_mask =
    publisher_info_->data_writer_->get_status_mask();

  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  if (callback) {
    switch (event_type) {
      case RMW_EVENT_LIVELINESS_LOST:
        {
          eprosima::fastdds::dds::LivelinessLostStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_liveliness_lost_status(current);
          lock_mutex.lock();
          if (!liveliness_changed_) {
            liveliness_lost_status_ = current;
          }

          if (liveliness_lost_status_.total_count_change > 0) {
            callback(user_data, liveliness_lost_status_.total_count_change);
            liveliness_lost_status_.total_count_change = 0;
          }
        }
        break;
      case RMW_EVENT_OFFERED_DEADLINE_MISSED:
        {
          eprosima::fastdds::dds::OfferedDeadlineMissedStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_offered_deadline_missed_status(current);
          lock_mutex.lock();
          if (!deadline_changed_) {
            offered_deadline_missed_status_ = current;
          }

          if (offered_deadline_missed_status_.total_count_change > 0) {
            callback(user_data, offered_deadline_missed_status_.total_count_change);
            offered_deadline_missed_status_.total_count_change = 0;
          }
        }
        break;
      case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE:
        {
          eprosima::fastdds::dds::OfferedIncompatibleQosStatus current;
          lock_mutex.unlock();
          publisher_info_->data_writer_->get_offered_incompatible_qos_status(current);
          lock_mutex.lock();
          if (!incompatible_qos_changed_) {
            incompatible_qos_status_ = current;
          }

          if (incompatible_qos_status_.total_count_change > 0) {
            callback(user_data, incompatible_qos_status_.total_count_change);
            incompatible_qos_status_.total_count_change = 0;
          }
        }
        break;
      case RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE:
        {
          // get_inconsistent_topic_status() operates on the Topic, not the DataWriter,
          // so it does not take the writer mutex; left querying under the lock.
          publisher_info_->data_writer_->get_topic()->get_inconsistent_topic_status(
            inconsistent_topic_status_);
          if (inconsistent_topic_status_.total_count_change > 0) {
            callback(user_data, inconsistent_topic_status_.total_count_change);
            inconsistent_topic_status_.total_count_change = 0;
          }
        }
        break;
      case RMW_EVENT_PUBLICATION_MATCHED:
        {
          if (matched_status_.total_count_change > 0) {
            callback(user_data, matched_status_.total_count_change);
            eprosima::fastdds::dds::PublicationMatchedStatus current;
            lock_mutex.unlock();
            publisher_info_->data_writer_->get_publication_matched_status(current);
            lock_mutex.lock();
            matched_status_ = current;
            matched_status_.total_count_change = 0;
            matched_status_.current_count_change = 0;
          }
        }
        break;
      default:
        break;
    }

    user_data_[event_type] = user_data;
    on_new_event_cb_[event_type] = callback;

    status_mask |= rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type);
  } else {
    user_data_[event_type] = nullptr;
    on_new_event_cb_[event_type] = nullptr;

    // publication_matched status should be kept enabled, since we need to
    // keep tracking matched subscriptions
    if (RMW_EVENT_PUBLICATION_MATCHED != event_type) {
      status_mask &= ~rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask(event_type);
    }
  }

  // set_listener() acquires the writer's internal mutex; release on_new_event_m_ first.
  lock_mutex.unlock();
  publisher_info_->data_writer_->set_listener(publisher_info_->data_writer_listener_, status_mask);
}

void RMWPublisherEvent::track_unique_subscription(eprosima::fastdds::rtps::GUID_t guid)
{
  std::lock_guard<std::mutex> lock(subscriptions_mutex_);
  subscriptions_.insert(guid);
}

void RMWPublisherEvent::untrack_unique_subscription(eprosima::fastdds::rtps::GUID_t guid)
{
  std::lock_guard<std::mutex> lock(subscriptions_mutex_);
  subscriptions_.erase(guid);
}

size_t RMWPublisherEvent::subscription_count() const
{
  std::lock_guard<std::mutex> lock(subscriptions_mutex_);
  return subscriptions_.size();
}

void RMWPublisherEvent::update_deadline(uint32_t total_count, uint32_t total_count_change)
{
  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  // Assign absolute values
  offered_deadline_missed_status_.total_count = total_count;
  // Accumulate deltas
  offered_deadline_missed_status_.total_count_change += total_count_change;

  deadline_changed_ = true;

  trigger_event(RMW_EVENT_OFFERED_DEADLINE_MISSED);
}

void RMWPublisherEvent::update_liveliness_lost(uint32_t total_count, uint32_t total_count_change)
{
  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  // Assign absolute values
  liveliness_lost_status_.total_count = total_count;
  // Accumulate deltas
  liveliness_lost_status_.total_count_change += total_count_change;

  liveliness_changed_ = true;

  trigger_event(RMW_EVENT_LIVELINESS_LOST);
}

void RMWPublisherEvent::update_offered_incompatible_qos(
  eprosima::fastdds::dds::QosPolicyId_t last_policy_id, uint32_t total_count,
  uint32_t total_count_change)
{
  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  // Assign absolute values
  incompatible_qos_status_.last_policy_id = last_policy_id;
  incompatible_qos_status_.total_count = total_count;
  // Accumulate deltas
  incompatible_qos_status_.total_count_change += total_count_change;

  incompatible_qos_changed_ = true;

  trigger_event(RMW_EVENT_OFFERED_QOS_INCOMPATIBLE);
}

void RMWPublisherEvent::update_inconsistent_topic(uint32_t total_count, uint32_t total_count_change)
{
  rcpputils::unique_lock<std::mutex> lock_mutex(on_new_event_m_);

  // Assign absolute values
  inconsistent_topic_status_.total_count = total_count;
  // Accumulate deltas
  inconsistent_topic_status_.total_count_change += total_count_change;

  inconsistent_topic_changed_ = true;

  trigger_event(RMW_EVENT_PUBLISHER_INCOMPATIBLE_TYPE);
}

void RMWPublisherEvent::update_matched(
  int32_t total_count,
  int32_t total_count_change,
  int32_t current_count,
  int32_t current_count_change)
{
  std::lock_guard<std::mutex> lock(on_new_event_m_);

  matched_status_.total_count = total_count;
  matched_status_.total_count_change += total_count_change;
  matched_status_.current_count = current_count;
  matched_status_.current_count_change += current_count_change;
  matched_changes_ = true;

  trigger_event(RMW_EVENT_PUBLICATION_MATCHED);
}

void RMWPublisherEvent::trigger_event(rmw_event_type_t event_type)
{
  if (on_new_event_cb_[event_type]) {
    on_new_event_cb_[event_type](user_data_[event_type], 1);
  }

  event_guard[event_type].set_trigger_value(true);
}