Adding graph and transaction classes

Author: ayrton04

fuse_core/CMakeLists.txt

@@ -43,12 +43,12 @@ add_library(
 #    src/async_sensor_model.cpp
 #    src/ceres_options.cpp
     src/constraint.cpp
-#    src/graph.cpp
+    src/graph.cpp
 #    src/graph_deserializer.cpp
     src/loss.cpp
     src/serialization.cpp
 #    src/timestamp_manager.cpp
-#    src/transaction.cpp
+    src/transaction.cpp
 #    src/transaction_deserializer.cpp
     src/uuid.cpp
     src/variable.cpp

fuse_core/include/fuse_core/graph.h

@@ -149,7 +149,7 @@ namespace fuse_core
 class Graph
 {
 public:
-  FUSE_SMART_PTR_ALIASES_ONLY(Graph);
+  FUSE_SMART_PTR_ALIASES_ONLY(Graph)
 
   /**
    * @brief A range of fuse_ros::Constraint objects
@@ -379,7 +379,7 @@ class Graph
    * @return            A Ceres Solver Summary structure containing information about the optimization process
    */
   virtual ceres::Solver::Summary optimizeFor(
-    const ros::Duration& max_optimization_time,
+    const rclcpp::Duration& max_optimization_time,
     const ceres::Solver::Options& options = ceres::Solver::Options()) = 0;
 
   /**

fuse_core/include/fuse_core/graph_deserializer.h

@@ -34,11 +34,11 @@
 #ifndef FUSE_CORE_GRAPH_DESERIALIZER_H
 #define FUSE_CORE_GRAPH_DESERIALIZER_H
 
-#include <fuse_msgs/SerializedGraph.h>
+#include <fuse_msgs/msg/SerializedGraph.hpp>
 #include <fuse_core/constraint.h>
 #include <fuse_core/graph.h>
 #include <fuse_core/variable.h>
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 
 
 namespace fuse_core

fuse_core/include/fuse_core/transaction.h

@@ -39,7 +39,7 @@
 #include <fuse_core/serialization.h>
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/range/any_range.hpp>
 #include <boost/serialization/access.hpp>
@@ -66,7 +66,7 @@ namespace fuse_core
 class Transaction
 {
 public:
-  FUSE_SMART_PTR_DEFINITIONS(Transaction);
+  FUSE_SMART_PTR_DEFINITIONS(Transaction)
 
   /**
    * @brief A range of Constraint::SharedPtr objects
@@ -78,13 +78,13 @@ class Transaction
   using const_constraint_range = boost::any_range<const Constraint, boost::forward_traversal_tag>;
 
   /**
-   * @brief A range of ros::Time objects
+   * @brief A range of rclcpp::Time objects
    *
    * 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 A range of UUID objects
@@ -107,12 +107,12 @@ class Transaction
   /**
    * @brief Read-only access to this transaction's timestamp
    */
-  const ros::Time& stamp() const { return stamp_; }
+  const rclcpp::Time& stamp() const { return stamp_; }
 
   /**
    * @brief Write access to this transaction's timestamp
    */
-  void stamp(const ros::Time& stamp) { stamp_ = stamp; }
+  void stamp(const rclcpp::Time& stamp) { stamp_ = stamp; }
 
   /**
    * @brief Read-only access to the set of timestamps involved in this transaction
@@ -127,15 +127,15 @@ class Transaction
    *
    * @return The minimum (oldest) timestamp.
    */
-  const ros::Time& minStamp() const;
+  const rclcpp::Time& minStamp() const;
 
   /**
    * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all involved
    * timestamps, if any
    *
    * @return The maximum (newest) timestamp.
    */
-  const ros::Time& maxStamp() const;
+  const rclcpp::Time& maxStamp() const;
 
   /**
    * @brief Read-only access to the added constraints
@@ -180,7 +180,7 @@ class Transaction
    *
    * @param[in] stamp The timestamp to be added
    */
-  void addInvolvedStamp(const ros::Time& stamp);
+  void addInvolvedStamp(const rclcpp::Time& stamp);
 
   /**
    * @brief Add a constraint to this transaction
@@ -282,10 +282,10 @@ class Transaction
   void deserialize(fuse_core::TextInputArchive& /* archive */);
 
 private:
-  ros::Time stamp_;  //!< The transaction message timestamp
+  rclcpp::Time stamp_;  //!< The transaction message timestamp
   std::vector<Constraint::SharedPtr> added_constraints_;  //!< The constraints to be added
   std::vector<Variable::SharedPtr> added_variables_;  //!< The variables to be added
-  std::set<ros::Time> involved_stamps_;  //!< The set of timestamps involved in this transaction
+  std::set<rclcpp::Time> involved_stamps_;  //!< The set of timestamps involved in this transaction
   std::vector<UUID> removed_constraints_;  //!< The constraint UUIDs to be removed
   std::vector<UUID> removed_variables_;  //!< The variable UUIDs to be removed
 

fuse_core/src/transaction.cpp

@@ -36,7 +36,7 @@
 #include <fuse_core/constraint.h>
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/range/empty.hpp>
@@ -49,7 +49,7 @@
 namespace fuse_core
 {
 
-const ros::Time& Transaction::minStamp() const
+const rclcpp::Time& Transaction::minStamp() const
 {
   if (involved_stamps_.empty())
   {
@@ -61,7 +61,7 @@ const ros::Time& Transaction::minStamp() const
   }
 }
 
-const ros::Time& Transaction::maxStamp() const
+const rclcpp::Time& Transaction::maxStamp() const
 {
   if (involved_stamps_.empty())
   {
@@ -73,7 +73,7 @@ const ros::Time& Transaction::maxStamp() const
   }
 }
 
-void Transaction::addInvolvedStamp(const ros::Time& stamp)
+void Transaction::addInvolvedStamp(const rclcpp::Time& stamp)
 {
   involved_stamps_.insert(stamp);
 }
@@ -236,11 +236,11 @@ void Transaction::merge(const Transaction& other, bool overwrite)
 
 void Transaction::print(std::ostream& stream) const
 {
-  stream << "Stamp: " << stamp_ << "\n";
+  stream << "Stamp: " << stamp_.seconds() << "\n";
   stream << "Involved Timestamps:\n";
   for (const auto& involved_stamp : involved_stamps_)
   {
-    stream << " - " << involved_stamp << "\n";
+    stream << " - " << involved_stamp.seconds() << "\n";
   }
   stream << "Added Variables:\n";
   for (const auto& added_variable : added_variables_)