From 9f9395e5146e8bf0bbeedd47dc953ec974ab4187 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 12 Nov 2022 10:41:20 +0800 Subject: [PATCH] fuse -> ROS 2 fuse_core : Nodes and Waitables (#284) Co-authored-by: Brett Downing Co-authored-by: Shane Loretz --- fuse_core/CMakeLists.txt | 326 ++++-------------- .../include/fuse_core/async_motion_model.h | 22 +- fuse_core/include/fuse_core/async_publisher.h | 28 +- .../include/fuse_core/async_sensor_model.h | 22 +- .../include/fuse_core/callback_wrapper.h | 75 +++- .../include/fuse_core/throttled_callback.h | 2 - fuse_core/src/async_motion_model.cpp | 71 ++-- fuse_core/src/async_publisher.cpp | 54 +-- fuse_core/src/async_sensor_model.cpp | 61 ++-- fuse_core/src/callback_wrapper.cpp | 133 +++++++ .../include/fuse_optimizers/batch_optimizer.h | 2 - .../fuse_optimizers/fixed_lag_smoother.h | 7 +- .../include/fuse_optimizers/optimizer.h | 18 +- fuse_optimizers/src/batch_optimizer.cpp | 17 +- fuse_optimizers/src/optimizer.cpp | 39 ++- fuse_optimizers/test/example_optimizer.h | 7 +- 16 files changed, 467 insertions(+), 417 deletions(-) create mode 100644 fuse_core/src/callback_wrapper.cpp diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 9071cbd3b..56d70aef9 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -29,21 +29,22 @@ include(boost-extras.cmake) ## fuse_core library add_library(${PROJECT_NAME} SHARED -# src/async_motion_model.cpp -# src/async_publisher.cpp -# src/async_sensor_model.cpp -# src/ceres_options.cpp - src/constraint.cpp - src/graph.cpp - src/graph_deserializer.cpp - src/loss.cpp - src/serialization.cpp - src/time.cpp - src/timestamp_manager.cpp - src/transaction.cpp - src/transaction_deserializer.cpp - src/uuid.cpp - src/variable.cpp + src/async_motion_model.cpp + src/async_publisher.cpp + src/async_sensor_model.cpp + src/callback_wrapper.cpp + # src/ceres_options.cpp + src/constraint.cpp + src/graph.cpp + src/graph_deserializer.cpp + src/loss.cpp + src/serialization.cpp + src/time.cpp + src/timestamp_manager.cpp + src/transaction.cpp + src/transaction_deserializer.cpp + src/uuid.cpp + src/variable.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" @@ -81,8 +82,58 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - # find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(test) + ament_add_gtest(test_constraint + test/test_constraint.cpp) + target_link_libraries(test_constraint ${PROJECT_NAME}) + target_include_directories(test_constraint PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_eigen + test/test_eigen.cpp) + target_link_libraries(test_eigen ${PROJECT_NAME}) + target_include_directories(test_eigen PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + # TODO(sloretz) fails to build: can't include ros/node_handle.h + # ament_add_gtest(test_local_parameterization + # test/test_local_parameterization.cpp) + # target_link_libraries(test_local_parameterization ${PROJECT_NAME}) + # target_include_directories(test_local_parameterization PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_loss + test/test_loss.cpp) + target_link_libraries(test_loss ${PROJECT_NAME}) + target_include_directories(test_loss PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + # TODO(sloretz) fails to build: invalid use of rclcpp::Time::get_clock_type() + # ament_add_gtest(test_message_buffer + # test/test_message_buffer.cpp) + # target_link_libraries(test_message_buffer ${PROJECT_NAME}) + # target_include_directories(test_message_buffer PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_timestamp_manager + test/test_timestamp_manager.cpp) + target_link_libraries(test_timestamp_manager ${PROJECT_NAME}) + target_include_directories(test_timestamp_manager PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_transaction + test/test_transaction.cpp) + target_link_libraries(test_transaction ${PROJECT_NAME}) + target_include_directories(test_transaction PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + # TODO(sloretz) fails to build: can't include ros/node_handle.h + # ament_add_gtest(test_util + # test/test_util.cpp) + # target_link_libraries(test_util ${PROJECT_NAME}) + # target_include_directories(test_util PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_uuid + test/test_uuid.cpp) + target_link_libraries(test_uuid ${PROJECT_NAME}) + target_include_directories(test_uuid PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + + ament_add_gtest(test_variable + test/test_variable.cpp) + target_link_libraries(test_variable ${PROJECT_NAME}) + target_include_directories(test_variable PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) endif() # TODO(CH3): Move this to the test directory and port to ROS 2 @@ -198,173 +249,6 @@ endif() # CXX_STANDARD_REQUIRED YES # ) -# # Constraint tests -# catkin_add_gtest(test_constraint -# test/test_constraint.cpp -# ) -# add_dependencies(test_constraint -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_constraint -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${CMAKE_CURRENT_SOURCE_DIR} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_constraint -# ${PROJECT_NAME} -# ) -# set_target_properties(test_constraint -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Eigen tests -# catkin_add_gtest(test_eigen -# test/test_eigen.cpp -# ) -# add_dependencies(test_eigen -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_eigen -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${CMAKE_CURRENT_SOURCE_DIR} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_eigen -# ${PROJECT_NAME} -# ) -# set_target_properties(test_eigen -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Local Parameterization tests -# catkin_add_gtest(test_local_parameterization -# test/test_local_parameterization.cpp -# ) -# target_include_directories(test_local_parameterization -# PRIVATE -# include -# ) -# target_link_libraries(test_local_parameterization -# ${PROJECT_NAME} -# ) -# set_target_properties(test_local_parameterization -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Loss tests -# catkin_add_gtest(test_loss -# test/test_loss.cpp -# ) -# add_dependencies(test_loss -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_loss -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${CMAKE_CURRENT_SOURCE_DIR} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_loss -# ${PROJECT_NAME} -# ) -# set_target_properties(test_loss -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Message Buffer Tests -# catkin_add_gtest(test_message_buffer -# test/test_message_buffer.cpp -# ) -# add_dependencies(test_message_buffer -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_message_buffer -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_message_buffer -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_message_buffer -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Timestamp Manager Tests -# catkin_add_gtest(test_timestamp_manager -# test/test_timestamp_manager.cpp -# ) -# add_dependencies(test_timestamp_manager -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_timestamp_manager -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_timestamp_manager -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_timestamp_manager -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Transaction tests -# catkin_add_gtest(test_transaction -# test/test_transaction.cpp -# ) -# add_dependencies(test_transaction -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_transaction -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${CMAKE_CURRENT_SOURCE_DIR} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_transaction -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_transaction -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - # # Parameter tests # add_rostest_gtest(test_parameter # test/parameter.test @@ -405,80 +289,6 @@ endif() # CXX_STANDARD 14 # CXX_STANDARD_REQUIRED YES # ) - -# # Util tests -# catkin_add_gtest(test_util -# test/test_util.cpp -# ) -# add_dependencies(test_util -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_util -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_util -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_util -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # UUID tests -# catkin_add_gtest(test_uuid -# test/test_uuid.cpp -# ) -# add_dependencies(test_uuid -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_uuid -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_uuid -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_uuid -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Variable tests -# catkin_add_gtest(test_variable -# test/test_variable.cpp -# ) -# add_dependencies(test_variable -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_variable -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${CMAKE_CURRENT_SOURCE_DIR} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_variable -# ${PROJECT_NAME} -# ) -# set_target_properties(test_variable -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) # endif() ############# diff --git a/fuse_core/include/fuse_core/async_motion_model.h b/fuse_core/include/fuse_core/async_motion_model.h index 425c92430..f41961d09 100644 --- a/fuse_core/include/fuse_core/async_motion_model.h +++ b/fuse_core/include/fuse_core/async_motion_model.h @@ -34,14 +34,13 @@ #ifndef FUSE_CORE_ASYNC_MOTION_MODEL_H #define FUSE_CORE_ASYNC_MOTION_MODEL_H -#include -#include #include + #include -// #include TODO(CH3): Uncomment when ready -#include -#include -#include +#include +#include + +#include #include @@ -171,13 +170,12 @@ class AsyncMotionModel : public MotionModel void stop() override; protected: - ros::CallbackQueue callback_queue_; //!< The local callback queue used for all subscriptions + std::shared_ptr callback_queue_; //!< The callback queue used for fuse internal callbacks std::string name_; //!< The unique name for this motion model instance - // rclcpp::Node::SharedPtr node_; //!< The node for this motion model (TODO(CH3): Uncomment when it's time) - ros::NodeHandle node_handle_; //!< A node handle in the global namespace using the local callback queue - ros::NodeHandle private_node_handle_; //!< A node handle in the private namespace using the local callback queue - ros::AsyncSpinner spinner_; //!< A single/multi-threaded spinner assigned to the local callback queue - + rclcpp::Node::SharedPtr node_; //!< The node for this motion model + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; //!< A single/multi-threaded spinner assigned to the local callback queue + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr waitables_interface_; + size_t executor_thread_count_; /** * @brief Constructor * diff --git a/fuse_core/include/fuse_core/async_publisher.h b/fuse_core/include/fuse_core/async_publisher.h index 1b8936342..a9711c47f 100644 --- a/fuse_core/include/fuse_core/async_publisher.h +++ b/fuse_core/include/fuse_core/async_publisher.h @@ -34,18 +34,22 @@ #ifndef FUSE_CORE_ASYNC_PUBLISHER_H #define FUSE_CORE_ASYNC_PUBLISHER_H -#include -#include #include + #include -// #include TODO(CH3): Uncomment when ready -#include -#include -#include +#include +#include +#include + +#include + +#include +#include #include + namespace fuse_core { @@ -134,12 +138,12 @@ class AsyncPublisher : public Publisher void stop() override; protected: - ros::CallbackQueue callback_queue_; //!< The local callback queue used for all subscriptions + std::shared_ptr callback_queue_; //!< The callback queue used for fuse internal callbacks std::string name_; //!< The unique name for this publisher instance - // rclcpp::Node::SharedPtr node_; //!< The node for this publisher (TODO(CH3): Uncomment when it's time) - ros::NodeHandle node_handle_; //!< A node handle in the global namespace using the local callback queue - ros::NodeHandle private_node_handle_; //!< A node handle in the private namespace using the local callback queue - ros::AsyncSpinner spinner_; //!< A single/multi-threaded spinner assigned to the local callback queue + rclcpp::Node::SharedPtr node_; //!< The node for this publisher + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; //!< A single/multi-threaded spinner assigned to the local callback queue + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr waitables_interface_; + size_t executor_thread_count_; /** * @brief Constructor @@ -153,7 +157,7 @@ class AsyncPublisher : public Publisher /** * @brief Perform any required initialization for the publisher * - * The node_handle_ and private_node_handle_ member variables will be completely initialized before this + * The node_ and member variables will be completely initialized before this * call occurs. Spinning of the callback queue will not begin until after the call to AsyncPublisher::onInit() * completes. * diff --git a/fuse_core/include/fuse_core/async_sensor_model.h b/fuse_core/include/fuse_core/async_sensor_model.h index 78ebd33d9..9935fbf14 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.h +++ b/fuse_core/include/fuse_core/async_sensor_model.h @@ -34,14 +34,13 @@ #ifndef FUSE_CORE_ASYNC_SENSOR_MODEL_H #define FUSE_CORE_ASYNC_SENSOR_MODEL_H -#include -#include #include + #include -// #include TODO(CH3): Uncomment when ready -#include -#include -#include +#include +#include + +//#include #include #include @@ -175,14 +174,13 @@ class AsyncSensorModel : public SensorModel void stop() override; protected: - // TODO(CH3): Add a node clock interface - ros::CallbackQueue callback_queue_; //!< The local callback queue used for all subscriptions + std::shared_ptr callback_queue_; //!< The callback queue used for fuse internal callbacks std::string name_; //!< The unique name for this sensor model instance - // rclcpp::Node::SharedPtr node_; //!< The node for this sensor model (TODO(CH3): Uncomment when it's time) - ros::NodeHandle node_handle_; //!< A node handle in the global namespace using the local callback queue - ros::NodeHandle private_node_handle_; //!< A node handle in the private namespace using the local callback queue - ros::AsyncSpinner spinner_; //!< A single/multi-threaded spinner assigned to the local callback queue + rclcpp::Node::SharedPtr node_; //!< The node for this sensor model + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; //!< A single/multi-threaded spinner assigned to the local callback queue TransactionCallback transaction_callback_; //!< The function to be executed every time a Transaction is "published" + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr waitables_interface_; + size_t executor_thread_count_; /** * @brief Constructor diff --git a/fuse_core/include/fuse_core/callback_wrapper.h b/fuse_core/include/fuse_core/callback_wrapper.h index 3b292fa28..977b56875 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.h +++ b/fuse_core/include/fuse_core/callback_wrapper.h @@ -31,14 +31,15 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef FUSE_CORE_CALLBACK_WRAPPER_H #define FUSE_CORE_CALLBACK_WRAPPER_H -#include - #include #include +#include +#include namespace fuse_core { @@ -76,18 +77,32 @@ namespace fuse_core * } * }; * + * auto callback_queue = std::make_shared( + * rclcpp::contexts::get_global_default_context()); + * node->get_node_waitables_interface()->add_waitable(callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); * MyClass my_object; * std::vector really_big_data(1000000); * auto callback = boost::make_shared >( * std::bind(&MyClass::processData, &my_object, std::ref(really_big_data))); + * callback_queue->add_callback(callback); * std::future result = callback->getFuture(); - * ros::getGlobalCallbackQueue()->addCallback(callback); * result.wait(); * RCLCPP_INFO_STREAM(rclcpp::get_logger("fuse"), "The result is: " << result.get()); * @endcode */ + +class CallbackWrapperBase +{ +public: + /** + * @brief Call this function. This is used by the callback queue. + */ + virtual void call() = 0; + +}; + template -class CallbackWrapper : public ros::CallbackInterface +class CallbackWrapper : public CallbackWrapperBase { public: using CallbackFunction = std::function; @@ -113,10 +128,9 @@ class CallbackWrapper : public ros::CallbackInterface /** * @brief Call this function. This is used by the callback queue. */ - CallResult call() override + void call() override { promise_.set_value(callback_()); - return Success; } private: @@ -127,13 +141,58 @@ class CallbackWrapper : public ros::CallbackInterface // Specialization to handle 'void' return types // Specifically, promise_.set_value(callback_()) does not work if callback_() returns void. template <> -inline ros::CallbackInterface::CallResult CallbackWrapper::call() +inline void CallbackWrapper::call() { callback_(); promise_.set_value(); - return Success; } + +class CallbackAdapter : public rclcpp::Waitable +{ +public: + + CallbackAdapter(std::shared_ptr context_ptr); + + /** + * @brief tell the CallbackGroup how many guard conditions are ready in this waitable + */ + size_t get_number_of_ready_guard_conditions() override; + + + /** + * @brief tell the CallbackGroup that this waitable is ready to execute anything + */ + bool is_ready(rcl_wait_set_t * wait_set) override; + + + /** + * @brief add_to_wait_set is called by rclcpp during NodeWaitables::add_waitable() and CallbackGroup::add_waitable() + waitable_ptr = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); + */ + void add_to_wait_set(rcl_wait_set_t * wait_set) override; + + std::shared_ptr< void > take_data() override; + + // TODO(CH3): check this against the threading model of the multi-threaded executor. + void execute(std::shared_ptr & data) override; + + void addCallback(const std::shared_ptr &callback); + + void addCallback(std::shared_ptr && callback); + + void removeAllCallbacks(); + + +private: + rcl_guard_condition_t gc_; //!< guard condition to drive the waitable + + std::mutex queue_mutex_; //!< mutex to allow this callback to be added to multiple callback groups simultaneously + std::deque > callback_queue_; +}; + + } // namespace fuse_core #endif // FUSE_CORE_CALLBACK_WRAPPER_H diff --git a/fuse_core/include/fuse_core/throttled_callback.h b/fuse_core/include/fuse_core/throttled_callback.h index a00444ef5..13f9032ec 100644 --- a/fuse_core/include/fuse_core/throttled_callback.h +++ b/fuse_core/include/fuse_core/throttled_callback.h @@ -34,8 +34,6 @@ #ifndef FUSE_CORE_THROTTLED_CALLBACK_H #define FUSE_CORE_THROTTLED_CALLBACK_H -//#include - #include #include diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index 3b31637be..e744cb2bc 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -36,11 +36,11 @@ #include #include #include -#include - -#include +#include +#include #include +#include #include #include @@ -50,7 +50,7 @@ namespace fuse_core AsyncMotionModel::AsyncMotionModel(size_t thread_count) : name_("uninitialized"), - spinner_(thread_count, &callback_queue_) + executor_thread_count_(thread_count) { } @@ -62,56 +62,79 @@ bool AsyncMotionModel::apply(Transaction& transaction) // Thus, it is functionally similar to a service callback, and should be a familiar pattern for ROS developers. // This function blocks until the queryCallback() call completes, thus enforcing that motion models are generated // in order. - auto callback = boost::make_shared>( + auto callback = std::make_shared>( std::bind(&AsyncMotionModel::applyCallback, this, std::ref(transaction))); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); - return result.get(); -} -void AsyncMotionModel::graphCallback(Graph::ConstSharedPtr graph) -{ - callback_queue_.addCallback( - boost::make_shared>(std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph))), - reinterpret_cast(this)); + return result.get(); } void AsyncMotionModel::initialize(const std::string& name) { // Initialize internal state name_ = name; - node_handle_.setCallbackQueue(&callback_queue_); - private_node_handle_ = ros::NodeHandle("~/" + name_); - private_node_handle_.setCallbackQueue(&callback_queue_); + 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); + + auto executor_options = rclcpp::ExecutorOptions(); + executor_options.context = ros_context; + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); + + callback_queue_ = std::make_shared(ros_context); + node_->get_node_waitables_interface()->add_waitable( + callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); // Call the derived onInit() function to perform implementation-specific initialization onInit(); - // Start the async spinner to service the local callback queue - spinner_.start(); + executor_->add_node(node_); +} + +void AsyncMotionModel::graphCallback(Graph::ConstSharedPtr graph) +{ + auto callback = std::make_shared>( + std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph)) + ); + // auto result = callback->getFuture(); // TODO(CH3): Circle back if we need this again + callback_queue_->addCallback(callback); + // result.wait(); } void AsyncMotionModel::start() { - auto callback = boost::make_shared>(std::bind(&AsyncMotionModel::onStart, this)); + auto callback = std::make_shared>( + std::bind(&AsyncMotionModel::onStart, this) + ); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } void AsyncMotionModel::stop() { - if (ros::ok()) + if (node_->get_node_base_interface()->get_context()->is_valid()) { - auto callback = boost::make_shared>(std::bind(&AsyncMotionModel::onStop, this)); + auto callback = std::make_shared>( + std::bind(&AsyncMotionModel::onStop, this) + ); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } else { - spinner_.stop(); + executor_->cancel(); + executor_->remove_node(node_); + onStop(); } } diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index 58054bd1c..f038a6c1e 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -33,16 +33,7 @@ */ #include -#include -#include -#include -#include - -#include - -#include -#include -#include +#include namespace fuse_core @@ -50,7 +41,7 @@ namespace fuse_core AsyncPublisher::AsyncPublisher(size_t thread_count) : name_("uninitialized"), - spinner_(thread_count, &callback_queue_) + executor_thread_count_(thread_count) { } @@ -58,46 +49,61 @@ void AsyncPublisher::initialize(const std::string& name) { // Initialize internal state name_ = name; - node_handle_.setCallbackQueue(&callback_queue_); - private_node_handle_ = ros::NodeHandle("~/" + name_); - private_node_handle_.setCallbackQueue(&callback_queue_); + 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); + + auto executor_options = rclcpp::ExecutorOptions(); + executor_options.context = ros_context; + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); + + callback_queue_ = std::make_shared(ros_context); + node_->get_node_waitables_interface()->add_waitable( + callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); // Call the derived onInit() function to perform implementation-specific initialization onInit(); - // Start the async spinner to service the local callback queue - spinner_.start(); + executor_->add_node(node_); } void AsyncPublisher::notify(Transaction::ConstSharedPtr transaction, Graph::ConstSharedPtr graph) { // Insert a call to the `notifyCallback` method into the internal callback queue. // This minimizes the time spent by the optimizer's thread calling this function. - auto callback = boost::make_shared>( + auto callback = std::make_shared>( std::bind(&AsyncPublisher::notifyCallback, this, std::move(transaction), std::move(graph))); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); } void AsyncPublisher::start() { - auto callback = boost::make_shared>(std::bind(&AsyncPublisher::onStart, this)); + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStart, this)); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } void AsyncPublisher::stop() { - if (ros::ok()) + if (node_->get_node_base_interface()->get_context()->is_valid()) { - auto callback = boost::make_shared>(std::bind(&AsyncPublisher::onStop, this)); + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStop, this)); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } else { - spinner_.stop(); + executor_->cancel(); + executor_->remove_node(node_); + onStop(); } } diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 069817b86..5ccfcb3b1 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -36,30 +36,22 @@ #include #include #include -#include - -#include #include #include #include +#include namespace fuse_core { AsyncSensorModel::AsyncSensorModel(size_t thread_count) : name_("uninitialized"), - spinner_(thread_count, &callback_queue_) + executor_thread_count_(thread_count) { } -void AsyncSensorModel::graphCallback(Graph::ConstSharedPtr graph) -{ - callback_queue_.addCallback( - boost::make_shared>(std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph))), - reinterpret_cast(this)); -} void AsyncSensorModel::initialize( const std::string& name, @@ -67,16 +59,39 @@ void AsyncSensorModel::initialize( { // Initialize internal state name_ = name; - node_handle_.setCallbackQueue(&callback_queue_); - private_node_handle_ = ros::NodeHandle("~/" + name_); - private_node_handle_.setCallbackQueue(&callback_queue_); + 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); + + auto executor_options = rclcpp::ExecutorOptions(); + executor_options.context = ros_context; + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); + + callback_queue_ = std::make_shared(ros_context); + node_->get_node_waitables_interface()->add_waitable( + callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); + transaction_callback_ = transaction_callback; // Call the derived onInit() function to perform implementation-specific initialization onInit(); // Start the async spinner to service the local callback queue - spinner_.start(); + executor_->add_node(node_); +} + +void AsyncSensorModel::graphCallback(Graph::ConstSharedPtr graph) +{ + auto callback = std::make_shared>( + std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph)) + ); + callback_queue_->addCallback(callback); } void AsyncSensorModel::sendTransaction(Transaction::SharedPtr transaction) @@ -86,24 +101,30 @@ void AsyncSensorModel::sendTransaction(Transaction::SharedPtr transaction) void AsyncSensorModel::start() { - auto callback = boost::make_shared>(std::bind(&AsyncSensorModel::onStart, this)); + auto callback = std::make_shared>( + std::bind(&AsyncSensorModel::onStart, this) + ); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } void AsyncSensorModel::stop() { - if (ros::ok()) + if (node_->get_node_base_interface()->get_context()->is_valid()) { - auto callback = boost::make_shared>(std::bind(&AsyncSensorModel::onStop, this)); + auto callback = std::make_shared>( + std::bind(&AsyncSensorModel::onStop, this) + ); auto result = callback->getFuture(); - callback_queue_.addCallback(callback, reinterpret_cast(this)); + callback_queue_->addCallback(callback); result.wait(); } else { - spinner_.stop(); + executor_->cancel(); + executor_->remove_node(node_); + onStop(); } } diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp new file mode 100644 index 000000000..6dadeb704 --- /dev/null +++ b/fuse_core/src/callback_wrapper.cpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Brett Downing + * 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 + +namespace fuse_core +{ + + CallbackAdapter::CallbackAdapter(std::shared_ptr context_ptr){ + + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options(); + + // Guard condition is used by the wait set to handle execute-or-not logic + gc_ = rcl_get_zero_initialized_guard_condition(); + if (RCL_RET_OK != rcl_guard_condition_init( + &gc_, context_ptr->get_rcl_context().get(), guard_condition_options) + ) { + throw std::runtime_error("Could not init guard condition for callback waitable."); + } + } + + /** + * @brief tell the CallbackGroup how many guard conditions are ready in this waitable + */ + size_t CallbackAdapter::get_number_of_ready_guard_conditions() { return 1;} + + /** + * @brief tell the CallbackGroup that this waitable is ready to execute anything + */ + bool CallbackAdapter::is_ready(rcl_wait_set_t * wait_set) { + (void) wait_set; + return !callback_queue_.empty(); + } + + /** + * @brief add_to_wait_set is called by rclcpp during NodeWaitables::add_waitable() and CallbackGroup::add_waitable() + waitable_ptr = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); + */ + void CallbackAdapter::add_to_wait_set(rcl_wait_set_t * wait_set) + { + if (RCL_RET_OK != rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL)) { + RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not add callback waitable to wait set."); + } + } + + /** + * @brief check the callback queue and return the next callback to run + * + */ + std::shared_ptr< void > CallbackAdapter::take_data(){ + std::shared_ptr cb_wrapper = nullptr; + // fetch the callback ptr and release the lock without spending time in the callback + { + std::lock_guard lock(queue_mutex_); + if(!callback_queue_.empty()){ + cb_wrapper = callback_queue_.front(); + callback_queue_.pop_front(); + } + } + return std::static_pointer_cast(cb_wrapper); + } + + /** + * @brief hook that allows the rclcpp::waitables interface to run the next callback + * + */ + void CallbackAdapter::execute(std::shared_ptr & data){ + if (!data) { + throw std::runtime_error("'data' is empty"); + } + std::static_pointer_cast(data)->call(); + } + + void CallbackAdapter::addCallback(const std::shared_ptr &callback){ + std::lock_guard lock(queue_mutex_); + callback_queue_.push_back(callback); + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo + } + } + + void CallbackAdapter::addCallback(std::shared_ptr && callback){ + std::lock_guard lock(queue_mutex_); + callback_queue_.push_back(std::move(callback)); + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo + } + } + + void CallbackAdapter::removeAllCallbacks(){ + std::lock_guard lock(queue_mutex_); + callback_queue_.clear(); + } + + +} // namespace fuse_core diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h index acab20493..c25ebe3c0 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h @@ -182,8 +182,6 @@ class BatchOptimizer : public Optimizer * * This callback checks if a current optimization cycle is still running. If not, a new optimization cycle is started. * If so, we simply wait for the next timer event to start another optimization cycle. - * - * @param event The ROS timer event metadata */ void optimizerTimerCallback(); diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index ffd7947af..deb4ba91f 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -39,7 +39,8 @@ #include #include #include -#include + +#include #include #include @@ -247,8 +248,6 @@ class FixedLagSmoother : public Optimizer * * This callback checks if a current optimization cycle is still running. If not, a new optimization cycle is started. * If so, we simply wait for the next timer event to start another optimization cycle. - * - * @param event The ROS timer event metadata */ void optimizerTimerCallback(); @@ -271,7 +270,7 @@ class FixedLagSmoother : public Optimizer bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); /** - * @brief Thread-safe read-only access to the optimizer start time + * @brief Thread-safe read-only access to the timestamp of the first transaction */ rclcpp::Time getStartTime() const { diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 2f87380dd..262ba98f5 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -42,8 +42,7 @@ #include #include #include -#include -// #include TODO(CH3): Uncomment when it's time +#include #include #include @@ -105,9 +104,9 @@ class Optimizer : public rclcpp::Node * @param[in] private_node_handle A node handle in the node's private namespace */ Optimizer( + rclcpp::NodeOptions options, fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + ); /** * @brief Destructor @@ -151,10 +150,6 @@ class Optimizer : public rclcpp::Node AssociatedMotionModels associated_motion_models_; //!< Tracks what motion models should be used for each sensor fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and constraints - // Ordering ROS objects with callbacks last - // TODO(CH3): Store clock interface - ros::NodeHandle node_handle_; //!< Node handle in the public namespace for subscribing and advertising - ros::NodeHandle private_node_handle_; //!< Node handle in the private namespace for reading configuration settings pluginlib::ClassLoader motion_model_loader_; //!< Pluginlib class loader for MotionModels MotionModels motion_models_; //!< The set of motion models, addressable by name pluginlib::ClassLoader publisher_loader_; //!< Pluginlib class loader for Publishers @@ -163,9 +158,12 @@ class Optimizer : public rclcpp::Node SensorModels sensor_models_; //!< The set of sensor models, addressable by name diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater - rclcpp::TimerBase::SharedPtr diagnostic_updater_timer_; //!< Diagnostic updater timer + rclcpp::TimerBase::SharedPtr diagnostic_updater_timer_; //!< Diagnostic updater timer double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds + std::shared_ptr callback_queue_; + + /** * @brief Callback fired every time a SensorModel plugin creates a new transaction * @@ -227,7 +225,7 @@ class Optimizer : public rclcpp::Node fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); - /** + /** * @brief Inject a transaction callback function into the global callback queue * * @param[in] sensor_name The name of the sensor that produced the Transaction diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index 3118a4b66..760319563 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -48,14 +48,13 @@ namespace fuse_optimizers BatchOptimizer::BatchOptimizer( rclcpp::NodeOptions options, - std::string node_name, fuse_core::Graph::UniquePtr graph ): - fuse_optimizers::Optimizer(options, node_name, std::move(graph)), + fuse_optimizers::Optimizer(options, std::move(graph)), combined_transaction_(fuse_core::Transaction::make_shared()), optimization_request_(false), - start_time_(rclcpp::Time::max()), // NOTE(CH3): ??? - started_(false) + start_time_(rclcpp::Time::max()), + started_(false) { params_.loadFromROS(private_node_handle); @@ -130,15 +129,19 @@ void BatchOptimizer::applyMotionModelsToQueue() void BatchOptimizer::optimizationLoop() { // Optimize constraints until told to exit - while (ros::ok()) + while (get_node_base_interface()->get_context()->is_valid()) { // Wait for the next signal to start the next optimization cycle { std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait(lock, [this]{ return optimization_request_ || !ros::ok(); }); // NOLINT + optimization_requested_.wait( + lock, + [this]{ + return optimization_request_ || !get_node_base_interface()->get_context()->is_valid(); + }); } // If a shutdown is requested, exit now. - if (!ros::ok()) + if (!get_node_base_interface()->get_context()->is_valid()) { break; } diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 7192c632e..44df06080 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -37,9 +37,6 @@ #include #include #include -#include -#include -#include #include @@ -80,28 +77,30 @@ struct PluginConfig XmlRpc::XmlRpcValue config; //!< The entire configuration, that might have additional optional parameters }; -Optimizer::Optimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : +// TODO(CH3): pass a rclcpp::Context, and a CallbackGroup +Optimizer::Optimizer(rclcpp::NodeOptions options, fuse_core::Graph::UniquePtr graph) : + Node("optimizer_node", options), graph_(std::move(graph)), - node_handle_(node_handle), - private_node_handle_(private_node_handle), motion_model_loader_("fuse_core", "fuse_core::MotionModel"), publisher_loader_("fuse_core", "fuse_core::Publisher"), sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_(node_handle_) + diagnostic_updater_(shared_from_this()), + callback_queue_(rclcpp::contexts::get_global_default_context()) { // Setup diagnostics updater - private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_, - diagnostic_updater_timer_period_); + // TODO(CH3): private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_, + // diagnostic_updater_timer_period_); diagnostic_updater_timer_ = this->create_timer( rclcpp::Duration::from_seconds(diagnostic_updater_timer_period_), std::bind(&diagnostic_updater::Updater::update, &diagnostic_updater_) ); - diagnostic_updater_.add(private_node_handle_.getNamespace(), this, &Optimizer::setDiagnostics); + //add a ros1 style callback queue so that transactions can be processed in the optimiser's executor + this->get_node_waitables_interface()->add_waitable( + callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); + + diagnostic_updater_.add(this->get_namespace(), this, &Optimizer::setDiagnostics); diagnostic_updater_.setHardwareID("fuse"); // Wait for a valid time before loading any of the plugins @@ -434,15 +433,15 @@ void Optimizer::injectCallback( // We are going to insert a call to the derived class's transactionCallback() method into the global callback queue. // This returns execution to the sensor's thread quickly by moving the transaction processing to the optimizer's // thread. And by using the existing ROS callback queue, we simplify the threading model of the optimizer. - ros::getGlobalCallbackQueue()->addCallback( - boost::make_shared>( - std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))), - reinterpret_cast(this)); + + auto callback = std::make_shared>( + std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))); + callback_queue_->addCallback(callback, (rclcpp::CallbackGroup::SharedPtr) nullptr); } void Optimizer::clearCallbacks() { - ros::getGlobalCallbackQueue()->removeByID(reinterpret_cast(this)); + callback_queue_->removeAllCallbacks(); } void Optimizer::startPlugins() @@ -489,7 +488,9 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat return; } - status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged"); + // TODO test for previous convergence success or failure + + status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimizer exists"); auto print_key = [](const std::string& result, const auto& entry) { return result + entry.first + ' '; }; diff --git a/fuse_optimizers/test/example_optimizer.h b/fuse_optimizers/test/example_optimizer.h index 0153c2955..9c771d5f1 100644 --- a/fuse_optimizers/test/example_optimizer.h +++ b/fuse_optimizers/test/example_optimizer.h @@ -49,9 +49,10 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer public: FUSE_SMART_PTR_DEFINITIONS(ExampleOptimizer) - ExampleOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) - : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) + ExampleOptimizer( + rclcpp::NodeOptions options, + fuse_core::Graph::UniquePtr graph = fuse_graphs::HashGraph::make_unique() + ) : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) { }