Moving variable sync from fuse_publishers to fuse_variables (#413)

Author: DavidLocus

fuse_models/include/fuse_models/odometry_2d_publisher.h

@@ -42,7 +42,7 @@
 #include <fuse_core/transaction.h>
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <fuse_publishers/stamped_variable_synchronizer.h>
+#include <fuse_variables/stamped_variable_synchronizer.h>
 
 #include <geometry_msgs/AccelWithCovarianceStamped.h>
 #include <nav_msgs/Odometry.h>
@@ -172,7 +172,7 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher
   /**
    * @brief Object that searches for the most recent common timestamp for a set of variables
    */
-  using Synchronizer = fuse_publishers::StampedVariableSynchronizer<fuse_variables::Orientation2DStamped,
+  using Synchronizer = fuse_variables::StampedVariableSynchronizer<fuse_variables::Orientation2DStamped,
                                                                     fuse_variables::Position2DStamped,
                                                                     fuse_variables::VelocityLinear2DStamped,
                                                                     fuse_variables::VelocityAngular2DStamped,

fuse_publishers/CMakeLists.txt

@@ -164,28 +164,4 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
-
-  # StampedVariableSynchronizer Tests
-  catkin_add_gtest(test_stamped_variable_synchronizer
-    test/test_stamped_variable_synchronizer.cpp
-  )
-  add_dependencies(test_stamped_variable_synchronizer
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_stamped_variable_synchronizer
-    PRIVATE
-      include
-  )
-  target_include_directories(test_stamped_variable_synchronizer
-    SYSTEM PRIVATE
-      ${catkin_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_stamped_variable_synchronizer
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_stamped_variable_synchronizer
-    PROPERTIES
-      CXX_STANDARD 17
-      CXX_STANDARD_REQUIRED YES
-  )
 endif()

fuse_publishers/include/fuse_publishers/pose_2d_publisher.h

@@ -34,7 +34,7 @@
 #ifndef FUSE_PUBLISHERS_POSE_2D_PUBLISHER_H
 #define FUSE_PUBLISHERS_POSE_2D_PUBLISHER_H
 
-#include <fuse_publishers/stamped_variable_synchronizer.h>
+#include <fuse_variables/stamped_variable_synchronizer.h>
 
 #include <fuse_core/async_publisher.h>
 #include <fuse_core/graph.h>
@@ -149,7 +149,7 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher
   void tfPublishTimerCallback(const ros::TimerEvent& event);
 
 protected:
-  using Synchronizer = StampedVariableSynchronizer<fuse_variables::Orientation2DStamped,
+  using Synchronizer = fuse_variables::StampedVariableSynchronizer<fuse_variables::Orientation2DStamped,
                                                    fuse_variables::Position2DStamped>;
 
   std::string base_frame_;  //!< The name of the robot's base_link frame

fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h

@@ -34,288 +34,9 @@
 #ifndef FUSE_PUBLISHERS_STAMPED_VARIABLE_SYNCHRONIZER_H
 #define FUSE_PUBLISHERS_STAMPED_VARIABLE_SYNCHRONIZER_H
 
-#include <fuse_core/graph.h>
-#include <fuse_core/fuse_macros.h>
-#include <fuse_core/transaction.h>
-#include <fuse_core/uuid.h>
-#include <fuse_core/variable.h>
-#include <fuse_variables/stamped.h>
-#include <ros/time.h>
+#include <fuse_variables/stamped_variable_synchronizer.h>
 
-#include <type_traits>
-
-
-namespace fuse_publishers
-{
-
-/**
- * @brief A utility class that finds the most recent timestamp shared by a set of stamped variables
- *
- * This is designed to be used by derived fuse_core::Publisher classes. The class remembers the last common timestamp
- * from the previous call, and attempts to find a more recent common timestamp using the provided transaction
- * information. If no common timestamp is found after searching the transaction, a full search of the graph will
- * be conducted. If no common timestamp is found after searching the full graph, a zero timestamp will be returned.
- *
- * The set of variable types are provided in the template parameters. e.g.
- * @code{.cpp}
- * auto synchronizer = StampedVariableSynchronizer<Orientation2DStamped, Position2DStamped>(device_id);
- * @endcode
- *
- * This class only functions with variables derived from the fuse_variables::Stamped base class.
- */
-template <typename ...Ts>
-class StampedVariableSynchronizer
-{
-public:
-  FUSE_SMART_PTR_DEFINITIONS(StampedVariableSynchronizer);
-  static const ros::Time TIME_ZERO;  //!< Constant representing a zero timestamp
-
-  /**
-   * @brief Construct a synchronizer object
-   *
-   * @param[in] device_id The device id to use for all variable types
-   */
-  explicit StampedVariableSynchronizer(const fuse_core::UUID& device_id = fuse_core::uuid::NIL);
-
-  /**
-   * @brief Find the latest timestamp for which variables of all the specified template types exist in the graph
-   *
-   * @param[in] transaction A fuse_core::Transaction object containing the changes to the graph since the last call
-   * @param[in] graph       The complete graph
-   * @return The latest timestamp shared by all requested variable types
-   */
-  ros::Time findLatestCommonStamp(const fuse_core::Transaction& transaction, const fuse_core::Graph& graph);
-
-private:
-  fuse_core::UUID device_id_;  //!< The device_id to use with the Stamped classes
-  ros::Time latest_common_stamp_;  //!< The previously discovered common stamp
-
-  /**
-   * @brief Search the variables in the provided range for more recent timestamps. Update the \p latest_common_stamp_
-   *        member variable if a newer common timestamp is found.
-   *
-   * @param[in] variable_range The collection of variables to test
-   * @param[in] graph          The complete graph, used to verify that all requested variables exist for a given time
-   */
-  template <typename VariableRange>
-  void updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph);
-};
-
-namespace detail
-{
-
-/**
- * @brief Some helper structs for testing properties of all types in a template parameter pack
- */
-template<bool...> struct bool_pack;
-template<bool ...bs>
-using all_true_helper = std::is_same<bool_pack<bs..., true>, bool_pack<true, bs...>>;
-
-/**
- * @brief Test if a property is true for all types in a template parameter pack. This is a type.
- */
-template <typename ...Ts>
-using all_true = all_true_helper<Ts::value...>;
-
-/**
- * @brief Test if a property is true for all types in a template parameter pack. This is a boolean value.
- */
-template<typename ...Ts>
-constexpr bool allTrue = all_true<Ts...>::value;
-
-/**
- * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a type.
- */
-template<typename T>
-using is_stamped = std::is_base_of<fuse_variables::Stamped, T>;
-
-/**
- * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a boolean value.
- */
-template<typename T>
-constexpr bool isStamped = is_stamped<T>::value;
-
-/**
- * @brief Test if a class is derived from the fuse_core::Variable base class. This is a type.
- */
-template<typename T>
-using is_variable = std::is_base_of<fuse_core::Variable, T>;
-
-/**
- * @brief Test if a class is derived from the fuse_core::Variable base class. This is a boolean value.
- */
-template<typename T>
-constexpr bool isVariable = is_variable<T>::value;
-
-/**
- * @brief Test if a class is derived from both the fuse_core::Variable base class and the fuse_variables::Stamped
- *        base class. This is a type.
- */
-template<typename T>
-struct is_stamped_variable
-{
-  static constexpr bool value = isStamped<T> && isVariable<T>;
-};
-
-/**
- * @brief Test if a class is derived from both the fuse_core::Variable base class and the fuse_variables::Stamped
- *        base class. This is a boolean value.
- */
-template<typename T>
-constexpr bool isStampedVariable = is_stamped_variable<T>::value;
-
-/**
- * @brief Test if all of the template parameter pack types are fuse_core::Variable and fuse_variables::Stamped.
- *        This is a type.
- */
-template <typename ...Ts>
-using all_stamped_variables = all_true<is_stamped_variable<Ts>...>;
-
-/**
- * @brief Test if all of the template parameter pack types are fuse_core::Variable and fuse_variables::Stamped.
- *        This is a boolean value.
- */
-template<typename ...Ts>
-constexpr bool allStampedVariables = all_stamped_variables<Ts...>::value;
-
-/**
- * @brief Test if instances of all the template parameter pack types exist in the graph
- *
- * This version accepts an empty parameter pack, and is used to terminate the recursive template parameter pack
- * expansion.
- *
- * This would be much easier to write in C++17 using 'if constexpr (sizeof...(Ts) > 0)'
- *
- * @param[in] graph     The complete graph, used to verify the existence of a variable
- * @param[in] stamp     The timestamp used to construct all variable types
- * @param[in] device_id The device id used to construct all variable types
- * @return True if all variables exist, false otherwise
- */
-template <typename...>
-struct all_variables_exist
-{
-  static bool value(const fuse_core::Graph& /*graph*/, const ros::Time& /*stamp*/, const fuse_core::UUID& /*device_id*/)
-  {
-    return true;
-  }
-};
-
-/**
- * @brief Test if instances of all the template parameter pack types exist in the graph
- *
- * This version accepts two or more template arguments. The template parameter pack is expanded recursively.
- *
- * @param[in] graph     The complete graph, used to verify the existence of a variable
- * @param[in] stamp     The timestamp used to construct all variable types
- * @param[in] device_id The device id used to construct all variable types
- * @return True if all variables exist, false otherwise
- */
-template <typename T, typename ...Ts>
-struct all_variables_exist<T, Ts...>
-{
-  static bool value(const fuse_core::Graph& graph, const ros::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);
-  }
-};
-
-/**
- * @brief Test if a given variable is included in the template parameter pack types
- *
- * This version accepts an empty parameter pack, and is used to terminate the recursive template parameter pack
- * expansion.
- *
- * This would be much easier to write in C++17 using 'if constexpr (sizeof...(Ts) > 0)'
- *
- * @param[in] variable The variable to check against the template parameter pack
- * @return True if the variable's type is part of the template parameter pack, false otherwise
- */
-template <typename...>
-struct is_variable_in_pack
-{
-  static bool value(const fuse_core::Variable& /*variable*/)
-  {
-    return false;
-  }
-};
-
-/**
- * @brief Test if a given variable is included in the template parameter pack types
- *
- * This version accepts two or more template arguments. The template parameter pack is expanded recursively.
- *
- * @param[in] variable The variable to check against the template parameter pack
- * @return True if the variable's type is part of the template parameter pack, false otherwise
- */
-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>
-const ros::Time StampedVariableSynchronizer<Ts...>::TIME_ZERO = ros::Time(0, 0);
-
-template <typename ...Ts>
-StampedVariableSynchronizer<Ts...>::StampedVariableSynchronizer(const fuse_core::UUID& device_id) :
-  device_id_(device_id),
-  latest_common_stamp_(TIME_ZERO)
-{
-  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>
-ros::Time StampedVariableSynchronizer<Ts...>::findLatestCommonStamp(
-  const fuse_core::Transaction& transaction,
-  const fuse_core::Graph& graph)
-{
-  // Clear the previous stamp if the variable was deleted
-  if (latest_common_stamp_ != TIME_ZERO &&
-      !detail::all_variables_exist<Ts...>::value(graph, latest_common_stamp_, device_id_))
-  {
-    latest_common_stamp_ = TIME_ZERO;
-  }
-  // 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 (latest_common_stamp_ == TIME_ZERO)
-  {
-    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
+#pragma message("The StampedVariableSynchronizer class has been moved to the fuse_variables package. " \
+                 "Please include that header instead.")
 
 #endif  // FUSE_PUBLISHERS_STAMPED_VARIABLE_SYNCHRONIZER_H

fuse_variables/CMakeLists.txt

@@ -97,6 +97,13 @@ install(
 #############
 
 if(CATKIN_ENABLE_TESTING)
+  set(test_depends
+    fuse_graphs
+  )
+  find_package(catkin REQUIRED COMPONENTS
+    ${build_depends}
+    ${test_depends}
+  )
   find_package(roslint REQUIRED)
   find_package(rostest REQUIRED)
 
@@ -583,4 +590,28 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
+
+  # StampedVariableSynchronizer Tests
+  catkin_add_gtest(test_stamped_variable_synchronizer
+    test/test_stamped_variable_synchronizer.cpp
+  )
+  add_dependencies(test_stamped_variable_synchronizer
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_stamped_variable_synchronizer
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_stamped_variable_synchronizer
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_stamped_variable_synchronizer
+    PROPERTIES
+      CXX_STANDARD 17
+      CXX_STANDARD_REQUIRED YES
+  )
 endif()