Program Listing for File stamped_variable_synchronizer.hpp

↰ Return to documentation for file (/tmp/ws/src/fuse/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp)

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_HPP_
#define FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_HPP_

#include <type_traits>

#include <fuse_core/graph.hpp>
#include <fuse_core/fuse_macros.hpp>
#include <fuse_core/transaction.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_core/variable.hpp>
#include <fuse_variables/stamped.hpp>

#include <rclcpp/time.hpp>


namespace fuse_publishers
{

template<typename ... Ts>
class StampedVariableSynchronizer
{
public:
  FUSE_SMART_PTR_DEFINITIONS(StampedVariableSynchronizer)


  explicit StampedVariableSynchronizer(const fuse_core::UUID & device_id = fuse_core::uuid::NIL);

  rclcpp::Time findLatestCommonStamp(
    const fuse_core::Transaction & transaction,
    const fuse_core::Graph & graph);

private:
  fuse_core::UUID device_id_;
  rclcpp::Time latest_common_stamp_;

  template<typename VariableRange>
  void updateTime(const VariableRange & variable_range, const fuse_core::Graph & graph);
};

namespace detail
{

template<bool...>
struct bool_pack;
template<bool ... bs>
using all_true_helper = std::is_same<bool_pack<bs..., true>, bool_pack<true, bs...>>;

template<typename ... Ts>
using all_true = all_true_helper<Ts::value...>;

template<typename ... Ts>
constexpr bool allTrue = all_true<Ts...>::value;

template<typename T>
using is_stamped = std::is_base_of<fuse_variables::Stamped, T>;

template<typename T>
constexpr bool isStamped = is_stamped<T>::value;

template<typename T>
using is_variable = std::is_base_of<fuse_core::Variable, T>;

template<typename T>
constexpr bool isVariable = is_variable<T>::value;

template<typename T>
struct is_stamped_variable
{
  static constexpr bool value = isStamped<T>&& isVariable<T>;
};

template<typename T>
constexpr bool isStampedVariable = is_stamped_variable<T>::value;

template<typename ... Ts>
using all_stamped_variables = all_true<is_stamped_variable<Ts>...>;

template<typename ... Ts>
constexpr bool allStampedVariables = all_stamped_variables<Ts...>::value;

template<typename ...>
struct all_variables_exist
{
  static bool value(
    const fuse_core::Graph & /*graph*/, const rclcpp::Time & /*stamp*/,
    const fuse_core::UUID & /*device_id*/)
  {
    return true;
  }
};

template<typename T, typename ... Ts>
struct all_variables_exist<T, Ts...>
{
  static bool value(
    const fuse_core::Graph & graph, const rclcpp::Time & stamp,
    const fuse_core::UUID & device_id)
  {
    return graph.variableExists(T(stamp, device_id).uuid()) &&
           all_variables_exist<Ts...>::value(graph, stamp, device_id);
  }
};

template<typename ...>
struct is_variable_in_pack
{
  static bool value(const fuse_core::Variable & /*variable*/)
  {
    return false;
  }
};

template<typename T, typename ... Ts>
struct is_variable_in_pack<T, Ts...>
{
  static bool value(const fuse_core::Variable & variable)
  {
    auto derived = dynamic_cast<const T *>(&variable);
    return static_cast<bool>(derived) ||
           is_variable_in_pack<Ts...>::value(variable);
  }
};

}  // namespace detail

template<typename ... Ts>
StampedVariableSynchronizer<Ts...>::StampedVariableSynchronizer(const fuse_core::UUID & device_id)
: device_id_(device_id),
  // NOTE(CH3): Uninitialized, for getting latest We use RCL_ROS_TIME so time comparisons are
  //            consistent
  latest_common_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME))
{
  static_assert(
    detail::allStampedVariables<Ts...>, "All synchronized types must be derived from both "
    "fuse_core::Variable and fuse_variable::Stamped.");
  static_assert(sizeof...(Ts) > 0, "At least one type must be specified.");
}

template<typename ... Ts>
rclcpp::Time StampedVariableSynchronizer<Ts...>::findLatestCommonStamp(
  const fuse_core::Transaction & transaction,
  const fuse_core::Graph & graph)
{
  // Clear the previous stamp if the variable was deleted
  if (0u != latest_common_stamp_.nanoseconds() &&
    !detail::all_variables_exist<Ts...>::value(graph, latest_common_stamp_, device_id_))
  {
    latest_common_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
  }
  // Search the transaction for more recent variables
  updateTime(transaction.addedVariables(), graph);
  // If no common timestamp was found, search the whole graph for the most recent variable set
  if (0u == latest_common_stamp_.nanoseconds()) {
    updateTime(graph.getVariables(), graph);
  }
  return latest_common_stamp_;
}

template<typename ... Ts>
template<typename VariableRange>
void StampedVariableSynchronizer<Ts...>::updateTime(
  const VariableRange & variable_range,
  const fuse_core::Graph & graph)
{
  for (const auto & candidate_variable : variable_range) {
    if (detail::is_variable_in_pack<Ts...>::value(candidate_variable)) {
      const auto & stamped_variable =
        dynamic_cast<const fuse_variables::Stamped &>(candidate_variable);
      if ((stamped_variable.stamp() > latest_common_stamp_) &&
        (stamped_variable.deviceId() == device_id_) &&
        (detail::all_variables_exist<Ts...>::value(graph, stamped_variable.stamp(), device_id_)))
      {
        latest_common_stamp_ = stamped_variable.stamp();
      }
    }
  }
}

}  // namespace fuse_publishers

#endif  // FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_HPP_