fixed_lag_smoother and batch_optimizer as components

fuse_optimizers/CMakeLists.txt

@@ -54,13 +54,33 @@ target_link_libraries(${PROJECT_NAME}
 )
 ament_target_dependencies(${PROJECT_NAME} diagnostic_updater)
 
-## batch_optimizer node
-add_executable(batch_optimizer_node src/batch_optimizer_node.cpp)
-target_link_libraries(batch_optimizer_node ${PROJECT_NAME})
+## batch_optimizer component
+add_library(batch_optimizer_component SHARED src/batch_optimizer_component.cpp)
+target_include_directories(batch_optimizer_component PRIVATE include)
+ament_target_dependencies(
+  batch_optimizer_component
+  SYSTEM
+  rclcpp_components
+)
+target_link_libraries(batch_optimizer_component ${PROJECT_NAME})
+
+rclcpp_components_register_node(
+  batch_optimizer_component PLUGIN "fuse_optimizers::BatchOptimizerComponent" EXECUTABLE
+  batch_optimizer_node)
+
+## fixed_lag_smoother component
+add_library(fixed_lag_smoother_component SHARED src/fixed_lag_smoother_component.cpp)
+target_include_directories(fixed_lag_smoother_component PRIVATE include)
+ament_target_dependencies(
+  fixed_lag_smoother_component
+  SYSTEM
+  rclcpp_components
+)
+target_link_libraries(fixed_lag_smoother_component ${PROJECT_NAME})
 
-## fixed_lag_smoother node
-add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp)
-target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME})
+rclcpp_components_register_node(
+  fixed_lag_smoother_component PLUGIN "fuse_optimizers::FixedLagSmootherComponent" EXECUTABLE
+  fixed_lag_smoother_node)
 
 #############
 ## Testing ##
@@ -76,7 +96,7 @@ endif()
 ## Install ##
 #############
 
-install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export
+install(TARGETS ${PROJECT_NAME} batch_optimizer_component fixed_lag_smoother_component EXPORT ${PROJECT_NAME}-export
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin

fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp

@@ -0,0 +1,20 @@
+#ifndef FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_
+#define FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "fuse_optimizers/batch_optimizer.hpp"
+
+namespace fuse_optimizers
+{
+  class BatchOptimizerComponent : public rclcpp::Node
+  {
+  public:
+    explicit BatchOptimizerComponent(const rclcpp::NodeOptions& options);
+
+
+  private:
+    std::shared_ptr<BatchOptimizer> batch_optimizer_;
+  };
+}  // namespace fuse_optimizers
+
+#endif  // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp

@@ -0,0 +1,19 @@
+#ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_
+#define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_
+
+#include <fuse_optimizers/fixed_lag_smoother.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace fuse_optimizers
+{
+  class FixedLagSmootherComponent : public rclcpp::Node
+  {
+  public:
+    explicit FixedLagSmootherComponent(const rclcpp::NodeOptions& options);
+
+  private:  
+    std::shared_ptr<FixedLagSmoother> fixed_lag_smoother_;
+  };
+}  // namespace fuse_optimizers
+
+#endif  // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_

fuse_optimizers/src/batch_optimizer_component.cpp

@@ -0,0 +1,14 @@
+#include "fuse_optimizers/batch_optimizer_component.hpp"
+namespace fuse_optimizers
+{
+  BatchOptimizerComponent::BatchOptimizerComponent(const rclcpp::NodeOptions& options)
+    : Node("batch_optimizer", options),
+      batch_optimizer_(std::make_shared<BatchOptimizer>(*this))
+  {
+    RCLCPP_INFO(get_logger(), "BatchOptimizerComponent constructed");
+  }
+}  // namespace fuse_optimizers
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizerComponent)

fuse_optimizers/src/fixed_lag_smoother_component.cpp

@@ -0,0 +1,15 @@
+#include "fuse_optimizers/fixed_lag_smoother_component.hpp"
+
+namespace fuse_optimizers
+{
+  FixedLagSmootherComponent::FixedLagSmootherComponent(const rclcpp::NodeOptions& options)
+    : Node("fixed_lag_smoother", options),
+      fixed_lag_smoother_(std::make_shared<FixedLagSmoother>(*this))
+  {
+    RCLCPP_INFO(get_logger(), "FixedLagSmootherComponent constructed");
+  }
+}  // namespace fuse_optimizers
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmootherComponent)