Adding timestamp manager

ayrton04 · ayrton04 · commit 532bc51ba8ae · 2022-05-18T17:11:02.000+01:00
diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt
@@ -49,7 +49,7 @@ add_library(
     src/graph_deserializer.cpp
     src/loss.cpp
     src/serialization.cpp
-#    src/timestamp_manager.cpp
+    src/timestamp_manager.cpp
     src/transaction.cpp
     src/transaction_deserializer.cpp
     src/uuid.cpp
diff --git a/fuse_core/include/fuse_core/timestamp_manager.h b/fuse_core/include/fuse_core/timestamp_manager.h
@@ -38,8 +38,7 @@
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/variable.h>
-#include <ros/duration.h>
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/range/any_range.hpp>
 
@@ -65,7 +64,7 @@ namespace fuse_core
 class TimestampManager
 {
 public:
-  FUSE_SMART_PTR_DEFINITIONS(TimestampManager);
+  FUSE_SMART_PTR_DEFINITIONS(TimestampManager)
 
   /**
    * @brief Function that generates motion model constraints between the requested timestamps
@@ -80,8 +79,8 @@ class TimestampManager
    * @param[out] variables       One or more variables at both the \p beginning_stamp and \p ending_stamp. The
    *                             variables should include initial values for the optimizer.
    */
-  using MotionModelFunction = std::function<void(const ros::Time& beginning_stamp,
-                                                 const ros::Time& ending_stamp,
+  using MotionModelFunction = std::function<void(const rclcpp::Time& beginning_stamp,
+                                                 const rclcpp::Time& ending_stamp,
                                                  std::vector<Constraint::SharedPtr>& constraints,
                                                  std::vector<Variable::SharedPtr>& variables)>;
 
@@ -90,9 +89,9 @@ class TimestampManager
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
-   * member. When dereferenced, an iterator returns a const ros::Time&.
+   * member. When dereferenced, an iterator returns a const rclcpp::Time&.
    */
-  using const_stamp_range = boost::any_range<const ros::Time, boost::forward_traversal_tag>;
+  using const_stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>;
 
   /**
    * @brief Constructor that accepts the motion model generator as a std::function object, probably constructed using
@@ -102,7 +101,7 @@ class TimestampManager
    * @param[in] buffer_length The length of the motion model history. If queries arrive involving timestamps
    *                          that are older than the buffer length, an exception will be thrown.
    */
-  explicit TimestampManager(MotionModelFunction generator, const ros::Duration& buffer_length = ros::DURATION_MAX);
+  explicit TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Constructor that accepts the motion model generator as a member function pointer and object pointer
@@ -116,12 +115,12 @@ class TimestampManager
    *                          that are older than the buffer length, an exception will be thrown.
    */
   template<class T>
-  TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                const ros::Time& ending_stamp,
+  TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                const rclcpp::Time& ending_stamp,
                                 std::vector<Constraint::SharedPtr>& constraints,
                                 std::vector<Variable::SharedPtr>& variables),
                    T* obj,
-                   const ros::Duration& buffer_length = ros::DURATION_MAX);
+                   const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Constructor that accepts the motion model generator as a const member function pointer and object pointer
@@ -135,12 +134,12 @@ class TimestampManager
    *                          that are older than the buffer length, an exception will be thrown.
    */
   template<class T>
-  TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                const ros::Time& ending_stamp,
+  TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                const rclcpp::Time& ending_stamp,
                                 std::vector<Constraint::SharedPtr>& constraints,
                                 std::vector<Variable::SharedPtr>& variables) const,
                    T* obj,
-                   const ros::Duration& buffer_length = ros::DURATION_MAX);
+                   const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Destructor
@@ -150,15 +149,15 @@ class TimestampManager
   /**
    * @brief Read-only access to the buffer length
    */
-  const ros::Duration& bufferLength() const
+  const rclcpp::Duration& bufferLength() const
   {
     return buffer_length_;
   }
 
   /**
    * @brief Write access to the buffer length
    */
-  void bufferLength(const ros::Duration& buffer_length)
+  void bufferLength(const rclcpp::Duration& buffer_length)
   {
     buffer_length_ = buffer_length;
   }
@@ -200,16 +199,16 @@ class TimestampManager
    */
   struct MotionModelSegment
   {
-    ros::Time beginning_stamp;
-    ros::Time ending_stamp;
+    rclcpp::Time beginning_stamp;
+    rclcpp::Time ending_stamp;
     std::vector<Constraint::SharedPtr> constraints;
     std::vector<Variable::SharedPtr> variables;
 
     MotionModelSegment() = default;
 
     MotionModelSegment(
-      const ros::Time& beginning_stamp,
-      const ros::Time& ending_stamp,
+      const rclcpp::Time& beginning_stamp,
+      const rclcpp::Time& ending_stamp,
       const std::vector<Constraint::SharedPtr>& constraints,
       const std::vector<Variable::SharedPtr>& variables) :
         beginning_stamp(beginning_stamp),
@@ -226,10 +225,10 @@ class TimestampManager
    * The MotionModelHistory will always contain all represented timestamps; the very last entry will be the ending
    * time of the previous MotionModelSegment, and the very last entry will be an empty MotionModelSegment.
    */
-  using MotionModelHistory = std::map<ros::Time, MotionModelSegment>;
+  using MotionModelHistory = std::map<rclcpp::Time, MotionModelSegment>;
 
   MotionModelFunction generator_;  //!< Users upplied function that generates motion model constraints
-  ros::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
+  rclcpp::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
                                  //!< will be removed from the motion model history
   MotionModelHistory motion_model_history_;  //!< Container that stores all previously generated motion models
 
@@ -243,8 +242,8 @@ class TimestampManager
    * @param[out] transaction     A transaction object to be updated with the changes caused by addSegment
    */
   void addSegment(
-    const ros::Time& beginning_stamp,
-    const ros::Time& ending_stamp,
+    const rclcpp::Time& beginning_stamp,
+    const rclcpp::Time& ending_stamp,
     Transaction& transaction);
 
   /**
@@ -271,7 +270,7 @@ class TimestampManager
    */
   void splitSegment(
       MotionModelHistory::iterator& iter,
-      const ros::Time& stamp,
+      const rclcpp::Time& stamp,
       Transaction& transaction);
 
   /**
@@ -281,12 +280,12 @@ class TimestampManager
 };
 
 template<class T>
-TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                                const ros::Time& ending_stamp,
+TimestampManager::TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                                const rclcpp::Time& ending_stamp,
                                                 std::vector<Constraint::SharedPtr>& constraints,
                                                 std::vector<Variable::SharedPtr>& variables),
                                    T* obj,
-                                   const ros::Duration& buffer_length) :
+                                   const rclcpp::Duration& buffer_length) :
   TimestampManager(std::bind(fp,
                              obj,
                              std::placeholders::_1,
@@ -298,12 +297,12 @@ TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp
 }
 
 template<class T>
-TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                                const ros::Time& ending_stamp,
+TimestampManager::TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                                const rclcpp::Time& ending_stamp,
                                                 std::vector<Constraint::SharedPtr>& constraints,
                                                 std::vector<Variable::SharedPtr>& variables) const,
                                    T* obj,
-                                   const ros::Duration& buffer_length) :
+                                   const rclcpp::Duration& buffer_length) :
   TimestampManager(std::bind(fp,
                              obj,
                              std::placeholders::_1,
diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp
@@ -36,8 +36,7 @@
 #include <fuse_core/constraint.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/variable.h>
-#include <ros/duration.h>
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/iterator/transform_iterator.hpp>
 
@@ -52,7 +51,7 @@
 namespace fuse_core
 {
 
-TimestampManager::TimestampManager(MotionModelFunction generator, const ros::Duration& buffer_length) :
+TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) :
   generator_(generator),
   buffer_length_(buffer_length)
 {
@@ -70,7 +69,7 @@ void TimestampManager::query(
   }
   // Verify the query is within the buffer length
   if ( (!motion_model_history_.empty())
-    && (buffer_length_ != ros::DURATION_MAX)
+    && (buffer_length_ != rclcpp::Duration::max())
     && (stamps.front() < motion_model_history_.begin()->first)
     && (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_)))
   {
@@ -79,7 +78,7 @@ void TimestampManager::query(
   // Create a list of all the required timestamps involved in motion model segments that must be created
   // Add all of the existing timestamps between the first and last input stamp
   Transaction motion_model_transaction;
-  std::set<ros::Time> augmented_stamps(stamps.begin(), stamps.end());
+  std::set<rclcpp::Time> augmented_stamps(stamps.begin(), stamps.end());
   auto first_stamp = *augmented_stamps.begin();
   auto last_stamp = *augmented_stamps.rbegin();
   {
@@ -99,14 +98,14 @@ void TimestampManager::query(
     }
   }
   // Convert the sequence of stamps into stamp pairs that must be generated
-  std::vector<std::pair<ros::Time, ros::Time>> stamp_pairs;
+  std::vector<std::pair<rclcpp::Time, rclcpp::Time>> stamp_pairs;
   {
     for (auto previous_iter = augmented_stamps.begin(), current_iter = std::next(augmented_stamps.begin());
          current_iter != augmented_stamps.end();
          ++previous_iter, ++current_iter)
     {
-      const ros::Time& previous_stamp = *previous_iter;
-      const ros::Time& current_stamp = *current_iter;
+      const rclcpp::Time& previous_stamp = *previous_iter;
+      const rclcpp::Time& current_stamp = *current_iter;
       // Check if the timestamp pair is exactly an existing pair. If so, don't add it.
       auto history_iter = motion_model_history_.lower_bound(previous_stamp);
       if ((history_iter != motion_model_history_.end()) &&
@@ -173,7 +172,7 @@ void TimestampManager::query(
 
 TimestampManager::const_stamp_range TimestampManager::stamps() const
 {
-  auto extract_stamp = +[](const MotionModelHistory::value_type& element) -> const ros::Time&
+  auto extract_stamp = +[](const MotionModelHistory::value_type& element) -> const rclcpp::Time&
   {
     return element.first;
   };
@@ -183,8 +182,8 @@ TimestampManager::const_stamp_range TimestampManager::stamps() const
 }
 
 void TimestampManager::addSegment(
-  const ros::Time& beginning_stamp,
-  const ros::Time& ending_stamp,
+  const rclcpp::Time& beginning_stamp,
+  const rclcpp::Time& ending_stamp,
   Transaction& transaction)
 {
   // Generate the set of constraints and variables to add
@@ -228,11 +227,11 @@ void TimestampManager::removeSegment(
 
 void TimestampManager::splitSegment(
     MotionModelHistory::iterator& iter,
-    const ros::Time& stamp,
+    const rclcpp::Time& stamp,
     Transaction& transaction)
 {
-  ros::Time removed_beginning_stamp = iter->second.beginning_stamp;
-  ros::Time removed_ending_stamp = iter->second.ending_stamp;
+  rclcpp::Time removed_beginning_stamp = iter->second.beginning_stamp;
+  rclcpp::Time removed_ending_stamp = iter->second.ending_stamp;
   // We need to remove the existing constraint.
   removeSegment(iter, transaction);
   // And add a new constraint from the beginning of the removed constraint to the provided stamp
@@ -244,17 +243,17 @@ void TimestampManager::splitSegment(
 void TimestampManager::purgeHistory()
 {
   // Purge any motion model segments that are more than buffer_length_ seconds older than the most recent entry
-  // A setting of ros::DURATION_MAX means "keep everything"
+  // A setting of rclcpp::Duration::max() means "keep everything"
   // And we want to keep at least one entry in motion model history, regardless of the stamps.
-  if ((buffer_length_ == ros::DURATION_MAX) || (motion_model_history_.size() <= 1))
+  if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1))
   {
     return;
   }
   // Continue to remove the first entry from the history until we:
   // (a) are left with only one entry, OR
   // (b) the time delta between the beginning and end is within the buffer_length_
   // We compare with the ending timestamp of each segment to be conservative
-  ros::Time ending_stamp = motion_model_history_.rbegin()->first;
+  rclcpp::Time ending_stamp = motion_model_history_.rbegin()->first;
   while ( (motion_model_history_.size() > 1)
       && ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_))
   {
