Port fuse_models

Signed-off-by: methylDragon <[email protected]>

doc/Variables.md

@@ -297,7 +297,7 @@ void print(std::ostream& stream = std::cout) const override
 {
   stream << type() << ":\n"
          << "  uuid: " << uuid_ << "\n"
-         << "  stamp: " << stamp_ << "\n"
+         << "  stamp: " << stamp_.nanoseconds() << "\n"
          << "  device_id: " << device_id_ << "\n"
          << "  size: " << data_.size() << "\n"
          << "  data:\n"

fuse_core/include/fuse_core/async_motion_model.hpp

@@ -142,7 +142,9 @@ class AsyncMotionModel : public MotionModel
    * @param[in] name A unique name to give this plugin instance
    * @throws runtime_error if already initialized
    */
-  void initialize(const std::string & name) override;
+  void initialize(
+    node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
+    const std::string & name) override;
 
   /**
    * @brief Get the unique name of this motion model
@@ -191,13 +193,15 @@ class AsyncMotionModel : public MotionModel
   std::shared_ptr<fuse_core::CallbackAdapter> callback_queue_;
 
   std::string name_;  //!< The unique name for this motion model instance
-  rclcpp::Node::SharedPtr node_;  //!< The node for this motion model
+
+  //! The node interfaces
+  node_interfaces::NodeInterfaces<node_interfaces::Base, node_interfaces::Waitables> interfaces_;
   rclcpp::CallbackGroup::SharedPtr cb_group_;  //!< Internal re-entrant callback group
 
   //! A single/multi-threaded executor assigned to the local callback queue
-  rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
+  rclcpp::Executor::SharedPtr executor_;
 
-  size_t executor_thread_count_;
+  size_t executor_thread_count_{1};
   std::thread spinner_;  //!< Internal thread for spinning the executor
   std::atomic<bool> initialized_ = false;  //!< True if instance has been fully initialized
 

fuse_core/include/fuse_core/async_sensor_model.hpp

@@ -40,6 +40,7 @@
 
 #include <fuse_core/callback_wrapper.hpp>
 #include <fuse_core/graph.hpp>
+#include <fuse_core/node_interfaces/node_interfaces.hpp>
 #include <fuse_core/sensor_model.hpp>
 #include <fuse_core/transaction.hpp>
 
@@ -127,6 +128,7 @@ class AsyncSensorModel : public SensorModel
    * @throws runtime_error if already initialized
    */
   void initialize(
+    node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
     const std::string & name,
     TransactionCallback transaction_callback) override;
 
@@ -193,15 +195,17 @@ class AsyncSensorModel : public SensorModel
   std::shared_ptr<fuse_core::CallbackAdapter> callback_queue_;
 
   std::string name_;  //!< The unique name for this sensor model instance
-  rclcpp::Node::SharedPtr node_;  //!< The node for this sensor model
+
+  //! The node interfaces
+  node_interfaces::NodeInterfaces<node_interfaces::Base, node_interfaces::Waitables> interfaces_;
   rclcpp::CallbackGroup::SharedPtr cb_group_;  //!< Internal re-entrant callback group
 
   //! A single/multi-threaded executor assigned to the local callback queue
-  rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
+  rclcpp::Executor::SharedPtr executor_;
 
   //! The function to be executed every time a Transaction is "published"
   TransactionCallback transaction_callback_;
-  size_t executor_thread_count_;
+  size_t executor_thread_count_{1};
   std::thread spinner_;  //!< Internal thread for spinning the executor
   std::atomic<bool> initialized_ = false;  //!< True if instance has been fully initialized
 

fuse_core/include/fuse_core/console.hpp

@@ -82,7 +82,8 @@ class DelayedThrottleFilter
    */
   bool isEnabled()
   {
-    const auto now = time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now());
+    const auto now = std::chrono::time_point_cast<std::chrono::milliseconds>(
+      std::chrono::system_clock::now());
 
     if (last_hit_.time_since_epoch().count() < 0.0) {
       last_hit_ = now;
@@ -102,8 +103,8 @@ class DelayedThrottleFilter
    */
   void reset()
   {
-    last_hit_ =
-      time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::from_time_t(-1));
+    last_hit_ = std::chrono::time_point_cast<std::chrono::milliseconds>(
+      std::chrono::system_clock::from_time_t(-1));
   }
 
 private:

fuse_core/include/fuse_core/motion_model.hpp

@@ -100,7 +100,9 @@ class MotionModel
    *
    * @param[in] name A unique name to give this plugin instance
    */
-  virtual void initialize(const std::string & name) = 0;
+  virtual void initialize(
+    node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
+    const std::string & name) = 0;
 
   /**
    * @brief Get the unique name of this motion model

fuse_core/include/fuse_core/parameter.hpp

@@ -277,8 +277,14 @@ inline fuse_core::Loss::SharedPtr loadLossConfig(
   const std::string & name
 )
 {
+  if (!interfaces.get_node_parameters_interface()->has_parameter(
+      name + ".type"))
+  {
+    return {};
+  }
+
   std::string loss_type;
-  getParamRequired(interfaces, name + "/type", loss_type);
+  getParamRequired(interfaces, name + ".type", loss_type);
 
   auto loss = fuse_core::createUniqueLoss(loss_type);
   loss->initialize(interfaces, interfaces.get_node_base_interface()->get_fully_qualified_name());

fuse_core/include/fuse_core/sensor_model.hpp

@@ -106,6 +106,7 @@ class SensorModel
   * @param[in] transaction_callback The function to call every time a transaction is published
   */
   virtual void initialize(
+    node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
     const std::string & name,
     TransactionCallback transaction_callback) = 0;
 

fuse_core/include/fuse_core/util.hpp

@@ -38,6 +38,7 @@
 #include <Eigen/Core>
 
 #include <cmath>
+#include <string>
 
 #include <rclcpp/logging.hpp>
 
@@ -149,6 +150,33 @@ Eigen::Matrix<T, 2, 2, Eigen::RowMajor> rotationMatrix2D(const T angle)
   return rotation;
 }
 
+/**
+ * @brief Create a compound ROS topic name from two components
+ *
+ * @param[in] a The first element
+ * @param[in] b The second element
+ * @return      The joined topic name
+ */
+inline
+std::string joinTopicName(std::string a, std::string b)
+{
+  if (a.empty()) {
+    return b;
+  }
+  if (b.empty()) {
+    return a;
+  }
+
+  if (a.back() == '/') {
+    a.pop_back();
+  }
+  if (b.front() == '/') {
+    b.erase(b.begin());
+  }
+
+  return a + "/" + b;
+}
+
 }  // namespace fuse_core
 
 #endif  // FUSE_CORE__UTIL_HPP_

fuse_core/src/async_motion_model.cpp

@@ -75,44 +75,43 @@ bool AsyncMotionModel::apply(Transaction & transaction)
   return result.get();
 }
 
-void AsyncMotionModel::initialize(const std::string & name)
+void AsyncMotionModel::initialize(
+  node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
+  const std::string & name)
 {
   if (initialized_) {
     throw std::runtime_error("Calling initialize on an already initialized AsyncMotionModel!");
   }
 
   // Initialize internal state
   name_ = name;
-  std::string node_namespace = "";
-
-  // TODO(CH3): Pass in the context or a node to get the context from
-  rclcpp::Context::SharedPtr ros_context = rclcpp::contexts::get_global_default_context();
-  auto node_options = rclcpp::NodeOptions();
-  node_options.context(ros_context);  // set a context to generate the node in
-
-  // TODO(CH3): Potentially pass in the optimizer node instead of spinning a new one
-  node_ = rclcpp::Node::make_shared(name_, node_namespace, node_options);
+  interfaces_ = interfaces;
 
+  auto context = interfaces_.get_node_base_interface()->get_context();
   auto executor_options = rclcpp::ExecutorOptions();
-  executor_options.context = ros_context;
-  executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(
-    executor_options,
-    executor_thread_count_
-  );
+  executor_options.context = context;
+
+  if (executor_thread_count_ == 1) {
+    executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options);
+  } else {
+    executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(
+      executor_options, executor_thread_count_);
+  }
 
-  callback_queue_ = std::make_shared<CallbackAdapter>(ros_context);
+  callback_queue_ = std::make_shared<CallbackAdapter>(context);
 
   // This callback group MUST be re-entrant in order to support parallelization
-  cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  node_->get_node_waitables_interface()->add_waitable(
-    callback_queue_, cb_group_);
+  cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(
+    rclcpp::CallbackGroupType::Reentrant, false);
+  interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_);
 
   // Call the derived onInit() function to perform implementation-specific initialization
   onInit();
 
   // Make sure the executor will service the given node
-  // TODO(sloretz) add just the callback group here when using Optimizer's Node
-  executor_->add_node(node_);
+  // We can add this without any guards because the callback group was set to not get automatically
+  // added to executors
+  executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface());
 
   // Start the executor
   spinner_ = std::thread(
@@ -155,7 +154,7 @@ void AsyncMotionModel::stop()
 {
   // Prefer to call onStop in executor's thread so downstream users don't have
   // to worry about threads in ROS callbacks when there's only 1 thread.
-  if (node_->get_node_base_interface()->get_context()->is_valid()) {
+  if (interfaces_.get_node_base_interface()->get_context()->is_valid()) {
     auto callback = std::make_shared<CallbackWrapper<void>>(
       std::bind(&AsyncMotionModel::onStop, this)
     );

fuse_core/src/async_publisher.cpp

@@ -52,14 +52,13 @@ void AsyncPublisher::initialize(
   node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
   const std::string & name)
 {
-  interfaces_ = interfaces;
-
   if (initialized_) {
     throw std::runtime_error("Calling initialize on an already initialized AsyncPublisher!");
   }
 
   // Initialize internal state
   name_ = name;  // NOTE(methylDragon): Used in derived classes
+  interfaces_ = interfaces;
 
   auto context = interfaces_.get_node_base_interface()->get_context();
   auto executor_options = rclcpp::ExecutorOptions();
@@ -72,8 +71,7 @@ void AsyncPublisher::initialize(
       executor_options, executor_thread_count_);
   }
 
-  callback_queue_ =
-    std::make_shared<CallbackAdapter>(context);
+  callback_queue_ = std::make_shared<CallbackAdapter>(context);
 
   // This callback group MUST be re-entrant in order to support parallelization
   cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(

fuse_core/src/async_sensor_model.cpp

@@ -57,6 +57,7 @@ AsyncSensorModel::~AsyncSensorModel()
 }
 
 void AsyncSensorModel::initialize(
+  node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
   const std::string & name,
   TransactionCallback transaction_callback)
 {
@@ -66,37 +67,35 @@ void AsyncSensorModel::initialize(
 
   // Initialize internal state
   name_ = name;
-  std::string node_namespace = "";
-
-  // TODO(CH3): Pass in the context or a node to get the context from
-  rclcpp::Context::SharedPtr ros_context = rclcpp::contexts::get_global_default_context();
-  auto node_options = rclcpp::NodeOptions();
-  node_options.context(ros_context);  // set a context to generate the node in
-
-  // TODO(CH3): Potentially pass in the optimizer node instead of spinning a new one
-  node_ = rclcpp::Node::make_shared(name_, node_namespace, node_options);
+  interfaces_ = interfaces;
 
+  auto context = interfaces_.get_node_base_interface()->get_context();
   auto executor_options = rclcpp::ExecutorOptions();
-  executor_options.context = ros_context;
-  executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(
-    executor_options,
-    executor_thread_count_);
+  executor_options.context = context;
+
+  if (executor_thread_count_ == 1) {
+    executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options);
+  } else {
+    executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(
+      executor_options, executor_thread_count_);
+  }
 
-  callback_queue_ = std::make_shared<CallbackAdapter>(ros_context);
+  callback_queue_ = std::make_shared<CallbackAdapter>(context);
 
   // This callback group MUST be re-entrant in order to support parallelization
-  cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  node_->get_node_waitables_interface()->add_waitable(
-    callback_queue_, cb_group_);
+  cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(
+    rclcpp::CallbackGroupType::Reentrant, false);
+  interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_);
 
   transaction_callback_ = transaction_callback;
 
   // Call the derived onInit() function to perform implementation-specific initialization
   onInit();
 
   // Make sure the executor will service the given node
-  // TODO(sloretz) add just the callback group here when using Optimizer's Node
-  executor_->add_node(node_);
+  // We can add this without any guards because the callback group was set to not get automatically
+  // added to executors
+  executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface());
 
   // Start the executor
   spinner_ = std::thread(
@@ -144,7 +143,7 @@ void AsyncSensorModel::stop()
 {
   // Prefer to call onStop in executor's thread so downstream users don't have
   // to worry about threads in ROS callbacks when there's only 1 thread.
-  if (node_->get_node_base_interface()->get_context()->is_valid()) {
+  if (interfaces_.get_node_base_interface()->get_context()->is_valid()) {
     auto callback = std::make_shared<CallbackWrapper<void>>(
       std::bind(&AsyncSensorModel::onStop, this)
     );

fuse_core/src/ceres_options.cpp

@@ -53,7 +53,7 @@ namespace fuse_core
 {
 
 // Helper function to get a namespace string with a '.' suffix, but only if not empty
-static std::string get_well_formatted_namespace_string(std::string ns)
+static std::string get_well_formatted_param_namespace_string(std::string ns)
 {
   return ns.empty() || ns.back() == '.' ? ns : ns + ".";
 }
@@ -70,7 +70,7 @@ void loadCovarianceOptionsFromROS(
 {
   rcl_interfaces::msg::ParameterDescriptor tmp_descr;
 
-  std::string ns = get_well_formatted_namespace_string(namespace_string);
+  std::string ns = get_well_formatted_param_namespace_string(namespace_string);
 
   // The sparse_linear_algebra_library_type field was added to ceres::Covariance::Options in version
   // 1.13.0, see https://github.com/ceres-solver/ceres-
@@ -127,7 +127,7 @@ void loadProblemOptionsFromROS(
 {
   rcl_interfaces::msg::ParameterDescriptor tmp_descr;
 
-  std::string ns = get_well_formatted_namespace_string(namespace_string);
+  std::string ns = get_well_formatted_param_namespace_string(namespace_string);
 
   tmp_descr.description = "trades memory for faster Problem::RemoveResidualBlock()";
   problem_options.enable_fast_removal = fuse_core::getParam(
@@ -162,7 +162,7 @@ void loadSolverOptionsFromROS(
 {
   rcl_interfaces::msg::ParameterDescriptor tmp_descr;
 
-  std::string ns = get_well_formatted_namespace_string(namespace_string);
+  std::string ns = get_well_formatted_param_namespace_string(namespace_string);
 
   // Minimizer options
   solver_options.minimizer_type =