Program Listing for File stamped_variable_synchronizer.hpp
↰ Return to documentation for file (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_