From 84c2edf7e52ab11d2e44be204b4997aa1c322adf Mon Sep 17 00:00:00 2001 From: Andrea Ostuni Date: Fri, 21 Jul 2023 10:48:31 +0200 Subject: [PATCH] feature: fixed_lag_smoother and batch_optimizer as components make composable optimizer nodes (dev) batch and fixed lag optimizer as components optimizer components refactor undo optimizer.cpp changes Update optimizer.hpp fuse optimizers cleanup Update README.md --- fuse_optimizers/CMakeLists.txt | 34 ++++++++++--- .../batch_optimizer_component.hpp | 20 ++++++++ .../fixed_lag_smoother_component.hpp | 19 +++++++ .../src/batch_optimizer_component.cpp | 14 ++++++ fuse_optimizers/src/batch_optimizer_node.cpp | 47 ------------------ .../src/fixed_lag_smoother_component.cpp | 15 ++++++ .../src/fixed_lag_smoother_node.cpp | 49 ------------------- 7 files changed, 95 insertions(+), 103 deletions(-) create mode 100644 fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp create mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp create mode 100644 fuse_optimizers/src/batch_optimizer_component.cpp delete mode 100644 fuse_optimizers/src/batch_optimizer_node.cpp create mode 100644 fuse_optimizers/src/fixed_lag_smoother_component.cpp delete mode 100644 fuse_optimizers/src/fixed_lag_smoother_node.cpp diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 6f21b1ce8..75fa7219d 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/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 diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_component.hpp new file mode 100644 index 000000000..37306090c --- /dev/null +++ b/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 batch_optimizer_; + }; +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_component.hpp new file mode 100644 index 000000000..a27aab423 --- /dev/null +++ b/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 +#include + +namespace fuse_optimizers +{ + class FixedLagSmootherComponent : public rclcpp::Node + { + public: + explicit FixedLagSmootherComponent(const rclcpp::NodeOptions& options); + + private: + std::shared_ptr fixed_lag_smoother_; + }; +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_COMPONENT_HPP_ \ No newline at end of file diff --git a/fuse_optimizers/src/batch_optimizer_component.cpp b/fuse_optimizers/src/batch_optimizer_component.cpp new file mode 100644 index 000000000..113792cfa --- /dev/null +++ b/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(*this)) + { + RCLCPP_INFO(get_logger(), "BatchOptimizerComponent constructed"); + } +} // namespace fuse_optimizers + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::BatchOptimizerComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp deleted file mode 100644 index e7d00b85f..000000000 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("batch_optimizer_node"); - auto optimizer = std::make_shared(*node); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/fuse_optimizers/src/fixed_lag_smoother_component.cpp b/fuse_optimizers/src/fixed_lag_smoother_component.cpp new file mode 100644 index 000000000..66f650a07 --- /dev/null +++ b/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(*this)) + { + RCLCPP_INFO(get_logger(), "FixedLagSmootherComponent constructed"); + } +} // namespace fuse_optimizers + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(fuse_optimizers::FixedLagSmootherComponent) \ No newline at end of file diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp deleted file mode 100644 index 7be160efa..000000000 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("fixed_lag_smoother_node"); - auto optimizer = std::make_shared(*node); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -}