From df291bd5099ae0f2c63ae9e87f40a6102bafdf65 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 1 Dec 2022 05:06:11 +0800 Subject: [PATCH] fuse -> ROS 2 fuse_core : Parameters and Tests (#286) Co-authored-by: Shane Loretz Co-authored-by: Ivor Wanders --- .gitignore | 3 + .../test/test_normal_delta_pose_2d.cpp | 2 +- .../test/test_normal_prior_pose_2d.cpp | 2 +- fuse_core/CMakeLists.txt | 214 +----- .../include/fuse_core/async_motion_model.h | 5 +- fuse_core/include/fuse_core/async_publisher.h | 4 +- .../include/fuse_core/async_sensor_model.h | 6 +- .../autodiff_local_parameterization.h | 5 +- fuse_core/include/fuse_core/ceres_options.h | 58 +- .../include/fuse_core/message_buffer_impl.h | 14 +- .../node_interfaces/node_interfaces.hpp | 153 ++++ .../node_interfaces_helpers.hpp | 365 +++++++++ fuse_core/include/fuse_core/parameter.h | 180 ++++- .../include/fuse_core/throttled_callback.h | 50 +- fuse_core/include/fuse_core/util.h | 1 - fuse_core/include/fuse_core/variable.h | 11 +- fuse_core/package.xml | 3 + fuse_core/src/async_motion_model.cpp | 19 + fuse_core/src/async_publisher.cpp | 18 + fuse_core/src/async_sensor_model.cpp | 17 + fuse_core/src/ceres_options.cpp | 717 +++++++++++++++--- fuse_core/test/CMakeLists.txt | 61 ++ fuse_core/test/async_motion_model.test | 4 - fuse_core/test/async_publisher.test | 4 - fuse_core/test/async_sensor_model.test | 4 - fuse_core/test/callback_wrapper.test | 4 - .../test/launch_tests/test_parameters.cpp | 242 ++++++ .../test/launch_tests/test_parameters.py | 48 ++ .../test/launch_tests/test_parameters.yaml | 28 + fuse_core/test/parameter.yaml | 26 - fuse_core/test/test_async_motion_model.cpp | 58 +- fuse_core/test/test_async_publisher.cpp | 44 +- fuse_core/test/test_async_sensor_model.cpp | 50 +- fuse_core/test/test_callback_wrapper.cpp | 68 +- fuse_core/test/test_constraint.cpp | 8 +- fuse_core/test/test_eigen.cpp | 6 - .../test/test_local_parameterization.cpp | 6 - fuse_core/test/test_loss.cpp | 8 +- fuse_core/test/test_message_buffer.cpp | 6 - fuse_core/test/test_parameter.cpp | 234 ------ fuse_core/test/test_throttled_callback.cpp | 210 +++-- fuse_core/test/test_timestamp_manager.cpp | 6 - fuse_core/test/test_transaction.cpp | 10 +- fuse_core/test/test_util.cpp | 7 - fuse_core/test/test_uuid.cpp | 6 - fuse_core/test/test_variable.cpp | 8 +- fuse_core/test/throttled_callback.test | 4 - .../benchmark/benchmark_create_problem.cpp | 2 +- fuse_graphs/test/test_hash_graph.cpp | 8 +- fuse_models/src/acceleration_2d.cpp | 5 +- fuse_models/src/imu_2d.cpp | 5 +- fuse_models/src/odometry_2d.cpp | 5 +- fuse_models/src/pose_2d.cpp | 5 +- fuse_models/src/twist_2d.cpp | 5 +- fuse_models/test/test_graph_ignition.cpp | 6 +- fuse_optimizers/test/test_optimizer.cpp | 4 +- fuse_publishers/src/serialized_publisher.cpp | 5 +- 57 files changed, 2088 insertions(+), 969 deletions(-) create mode 100644 fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp create mode 100644 fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp create mode 100644 fuse_core/test/CMakeLists.txt delete mode 100644 fuse_core/test/async_motion_model.test delete mode 100644 fuse_core/test/async_publisher.test delete mode 100644 fuse_core/test/async_sensor_model.test delete mode 100644 fuse_core/test/callback_wrapper.test create mode 100644 fuse_core/test/launch_tests/test_parameters.cpp create mode 100755 fuse_core/test/launch_tests/test_parameters.py create mode 100644 fuse_core/test/launch_tests/test_parameters.yaml delete mode 100644 fuse_core/test/parameter.yaml delete mode 100644 fuse_core/test/test_parameter.cpp delete mode 100644 fuse_core/test/throttled_callback.test diff --git a/.gitignore b/.gitignore index 259148fa1..60cf591f7 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,6 @@ *.exe *.out *.app + +# Python compiled bytecode files +*.pyc diff --git a/fuse_constraints/test/test_normal_delta_pose_2d.cpp b/fuse_constraints/test/test_normal_delta_pose_2d.cpp index 69b564c10..be71e273e 100644 --- a/fuse_constraints/test/test_normal_delta_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_delta_pose_2d.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include "cost_function_gtest.h" #include #include diff --git a/fuse_constraints/test/test_normal_prior_pose_2d.cpp b/fuse_constraints/test/test_normal_prior_pose_2d.cpp index 811010ddb..ad4a99c26 100644 --- a/fuse_constraints/test/test_normal_prior_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_2d.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include "cost_function_gtest.h" #include #include diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 56d70aef9..576aa8385 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(fuse_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rcl_interfaces REQUIRED) @@ -28,12 +28,12 @@ include(boost-extras.cmake) ########### ## fuse_core library -add_library(${PROJECT_NAME} SHARED +add_library(${PROJECT_NAME} src/async_motion_model.cpp src/async_publisher.cpp src/async_sensor_model.cpp src/callback_wrapper.cpp - # src/ceres_options.cpp + src/ceres_options.cpp src/constraint.cpp src/graph.cpp src/graph_deserializer.cpp @@ -82,215 +82,9 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - 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}) + add_subdirectory(test) endif() -# TODO(CH3): Move this to the test directory and port to ROS 2 -# if(CATKIN_ENABLE_TESTING) -# find_package(roslint REQUIRED) -# find_package(rostest REQUIRED) - -# # Lint tests -# set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") -# roslint_cpp() -# roslint_add_test() - -# # AsyncMotionModel tests -# add_rostest_gtest(test_async_motion_model -# test/async_motion_model.test -# test/test_async_motion_model.cpp -# ) -# add_dependencies(test_async_motion_model -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_async_motion_model -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_async_motion_model -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_async_motion_model -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # AsyncPublisher tests -# add_rostest_gtest(test_async_publisher -# test/async_publisher.test -# test/test_async_publisher.cpp -# ) -# add_dependencies(test_async_publisher -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_async_publisher -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_async_publisher -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_async_publisher -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # AsyncSensorModel tests -# add_rostest_gtest(test_async_sensor_model -# test/async_sensor_model.test -# test/test_async_sensor_model.cpp -# ) -# add_dependencies(test_async_sensor_model -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_async_sensor_model -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_async_sensor_model -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_async_sensor_model -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # CallbackWrapper tests -# add_rostest_gtest(test_callback_wrapper -# test/callback_wrapper.test -# test/test_callback_wrapper.cpp -# ) -# add_dependencies(test_callback_wrapper -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_callback_wrapper -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_callback_wrapper -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_callback_wrapper -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Parameter tests -# add_rostest_gtest(test_parameter -# test/parameter.test -# test/test_parameter.cpp -# ) -# add_dependencies(test_parameter -# ${catkin_EXPORTED_TARGETS} -# ) -# target_include_directories(test_parameter -# PRIVATE -# include -# ${Boost_INCLUDE_DIRS} -# ${catkin_INCLUDE_DIRS} -# ${CERES_INCLUDE_DIRS} -# ${EIGEN3_INCLUDE_DIRS} -# ) -# target_link_libraries(test_parameter -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_parameter -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) - -# # Throttle callback test -# add_rostest_gtest( -# test_throttled_callback -# test/throttled_callback.test -# test/test_throttled_callback.cpp -# ) -# target_link_libraries(test_throttled_callback -# ${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ) -# set_target_properties(test_throttled_callback -# PROPERTIES -# CXX_STANDARD 14 -# CXX_STANDARD_REQUIRED YES -# ) -# endif() - ############# ## Install ## ############# diff --git a/fuse_core/include/fuse_core/async_motion_model.h b/fuse_core/include/fuse_core/async_motion_model.h index f41961d09..48f0806e8 100644 --- a/fuse_core/include/fuse_core/async_motion_model.h +++ b/fuse_core/include/fuse_core/async_motion_model.h @@ -90,7 +90,7 @@ class AsyncMotionModel : public MotionModel /** * @brief Destructor */ - virtual ~AsyncMotionModel() = default; + virtual ~AsyncMotionModel(); /** * @brief Augment a transaction object such that all involved timestamps are connected by motion model constraints. @@ -174,8 +174,9 @@ class AsyncMotionModel : public MotionModel std::string name_; //!< The unique name for this motion model instance 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_; + std::thread spinner_; //!< Internal thread for spinning the executor + /** * @brief Constructor * diff --git a/fuse_core/include/fuse_core/async_publisher.h b/fuse_core/include/fuse_core/async_publisher.h index a9711c47f..fff6e8821 100644 --- a/fuse_core/include/fuse_core/async_publisher.h +++ b/fuse_core/include/fuse_core/async_publisher.h @@ -73,7 +73,7 @@ class AsyncPublisher : public Publisher /** * @brief Destructor */ - virtual ~AsyncPublisher() = default; + virtual ~AsyncPublisher(); /** * @brief Initialize the AsyncPublisher object @@ -142,8 +142,8 @@ class AsyncPublisher : public Publisher std::string name_; //!< The unique name for this publisher instance 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_; + std::thread spinner_; //!< Internal thread for spinning the executor /** * @brief Constructor diff --git a/fuse_core/include/fuse_core/async_sensor_model.h b/fuse_core/include/fuse_core/async_sensor_model.h index 9935fbf14..9d244dc1f 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.h +++ b/fuse_core/include/fuse_core/async_sensor_model.h @@ -40,8 +40,6 @@ #include #include -//#include - #include #include @@ -92,7 +90,7 @@ class AsyncSensorModel : public SensorModel /** * @brief Destructor */ - virtual ~AsyncSensorModel() = default; + virtual ~AsyncSensorModel(); /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -179,8 +177,8 @@ class AsyncSensorModel : public SensorModel 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_; + std::thread spinner_; //!< Internal thread for spinning the executor /** * @brief Constructor diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.h b/fuse_core/include/fuse_core/autodiff_local_parameterization.h index b2df32c9f..9941c43b1 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.h +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.h @@ -190,7 +190,7 @@ bool AutoDiffLocalParameterization ::Differentiate(*plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( + return ceres::internal::AutoDifferentiate>( *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #endif } @@ -217,7 +217,8 @@ bool AutoDiffLocalParameterization ::Differentiate(*minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( + using StaticParameters = ceres::internal::StaticParameterDims; + return ceres::internal::AutoDifferentiate( *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #endif } diff --git a/fuse_core/include/fuse_core/ceres_options.h b/fuse_core/include/fuse_core/ceres_options.h index b02840f22..d774d01f4 100644 --- a/fuse_core/include/fuse_core/ceres_options.h +++ b/fuse_core/include/fuse_core/ceres_options.h @@ -35,9 +35,10 @@ #define FUSE_CORE_CERES_OPTIONS_H #include +#include #include -#include +#include #include #include @@ -183,25 +184,34 @@ CERES_OPTION_STRING_DEFINITIONS(TrustRegionStrategyType) CERES_OPTION_STRING_DEFINITIONS(VisibilityClusteringType) /** - * @brief Helper function that loads a Ceres Option (e.g. ceres::LinearSolverType) value from the parameter server + * @brief Helper function that loads and validates a Ceres Option (e.g. ceres::LinearSolverType) value from the parameter server * - * @param[in] node_handle - The node handle used to load the parameter + * @param[in] interfaces - The node interfaces used to load the parameter * @param[in] parameter_name - The parameter name to load * @param[in] default_value - A default value to use if the provided parameter name does not exist * @return The loaded (or default) value */ -template // TODO(CH3): Replace nh with a node -T getParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, const T& default_value) +template +T declareCeresParam( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& parameter_name, + const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()) { const std::string default_string_value{ ToString(default_value) }; std::string string_value; - node_handle.param(parameter_name, string_value, default_string_value); + string_value = getParam(interfaces, parameter_name, default_string_value, parameter_descriptor); T value; if (!FromString(string_value, &value)) { - RCLCPP_WARN_STREAM(node->get_logger(), + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), "The requested " << parameter_name << " (" << string_value << ") is not supported. Using the default value (" << default_string_value << ") instead."); @@ -214,26 +224,48 @@ T getParam(const ros::NodeHandle& node_handle, const std::string& parameter_name /** * @brief Populate a ceres::Covariance::Options object with information from the parameter server * - * @param[in] nh - A node handle in a namespace containing ceres::Covariance::Options settings + * @param[in] interfaces - Node interfaces for a node in a namespace containing ceres::Covariance::Options settings * @param[out] covariance_options - The ceres::Covariance::Options object to update + * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ -void loadCovarianceOptionsFromROS(const ros::NodeHandle& nh, ceres::Covariance::Options& covariance_options); +void loadCovarianceOptionsFromROS( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + ceres::Covariance::Options& covariance_options, + const std::string& namespace_string = std::string()); /** * @brief Populate a ceres::Problem::Options object with information from the parameter server * - * @param[in] nh - A node handle in a namespace containing ceres::Problem::Options settings + * @param[in] interfaces - Node interfaces for a node in a namespace containing ceres::Problem::Options settings * @param[out] problem_options - The ceres::Problem::Options object to update + * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ -void loadProblemOptionsFromROS(const ros::NodeHandle& nh, ceres::Problem::Options& problem_options); +void loadProblemOptionsFromROS( + node_interfaces::NodeInterfaces< + node_interfaces::Parameters + > interfaces, + ceres::Problem::Options& problem_options, + const std::string& namespace_string = std::string()); /** * @brief Populate a ceres::Solver::Options object with information from the parameter server * - * @param[in] nh - A node handle in a namespace containing ceres::Solver::Options settings + * @param[in] interfaces - Node interfaces for a node in a namespace containing ceres::Solver::Options settings * @param[out] solver_options - The ceres::Solver::Options object to update + * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ -void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& solver_options); +void loadSolverOptionsFromROS( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + ceres::Solver::Options& solver_options, + const std::string& namespace_string = std::string()); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/message_buffer_impl.h b/fuse_core/include/fuse_core/message_buffer_impl.h index 12453d9e0..2f236019c 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.h +++ b/fuse_core/include/fuse_core/message_buffer_impl.h @@ -71,9 +71,9 @@ typename MessageBuffer::message_range MessageBuffer::query( if (ending_stamp < beginning_stamp) { std::stringstream beginning_time_ss; - beginning_time_ss << beginning_stamp; + beginning_time_ss << beginning_stamp.seconds(); std::stringstream ending_time_ss; - ending_time_ss << ending_stamp; + ending_time_ss << ending_stamp.seconds(); throw std::invalid_argument("The beginning_stamp (" + beginning_time_ss.str() + ") must be less than or equal to " "the ending_stamp (" + ending_time_ss.str() + ")."); } @@ -81,7 +81,7 @@ typename MessageBuffer::message_range MessageBuffer::query( if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || (ending_stamp > buffer_.back().first)) { std::stringstream requested_time_range_ss; - requested_time_range_ss << "(" << beginning_stamp << ", " << ending_stamp << ")"; + requested_time_range_ss << "(" << beginning_stamp.seconds() << ", " << ending_stamp.seconds() << ")"; std::stringstream available_time_range_ss; if (buffer_.empty()) { @@ -89,7 +89,8 @@ typename MessageBuffer::message_range MessageBuffer::query( } else { - available_time_range_ss << "(" << buffer_.front().first << ", " << buffer_.back().first << ")"; + available_time_range_ss << "(" << buffer_.front().first.seconds() << ", " + << buffer_.back().first.seconds() << ")"; } throw std::out_of_range("The requested time range " + requested_time_range_ss.str() + " is outside the available " "time range " + available_time_range_ss.str() + "."); @@ -141,11 +142,12 @@ void MessageBuffer::purgeHistory() // Compute the expiration time carefully, as ROS can't handle negative times const auto& ending_stamp = buffer_.back().first; + rclcpp::Time expiration_time; if (ending_stamp.seconds() > buffer_length_.seconds()) { - auto expiration_time = ending_stamp - buffer_length_; + expiration_time = ending_stamp - buffer_length_; } else { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. - auto expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type); + expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } // Remove buffer elements before the expiration time. diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp new file mode 100644 index 000000000..f9c6bea8c --- /dev/null +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -0,0 +1,153 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HPP_ +#define FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HPP_ + +#include + +#include "fuse_core/node_interfaces/node_interfaces_helpers.hpp" + +namespace fuse_core +{ +namespace node_interfaces +{ + +/// TODO(CH3): Remove this once https://github.com/ros2/rclcpp/pull/2041 is merged and released + +/// A helper class for aggregating node interfaces +template +class NodeInterfaces + : public std::enable_shared_from_this>, public InterfaceTs ... +{ + static_assert(0 != sizeof ...(InterfaceTs), "Template parameters must be populated!"); + +public: + RCLCPP_SMART_PTR_DEFINITIONS(NodeInterfaces) + + /// Create a new NodeInterfaces object with no bound node interfaces. + NodeInterfaces() + : InterfaceTs()... {} + + /// Create a new NodeInterfaces object bound with the passed in node-like object's interfaces. + /** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * You may use any of the available support classes in + * node_interfaces/node_interfaces_helpers.hpp: + * - Base: Supports NodeBaseInterface + * - Clock: Supports NodeClockInterface + * - Graph: Supports NodeGraphInterface + * - Logging: Supports NodeLoggingInterface + * - Parameters: Supports NodeParametersInterface + * - Services: Supports NodeServicesInterface + * - TimeSource: Supports NodeTimeSourceInterface + * - Timers: Supports NodeTimersInterface + * - Topics: Supports NodeTopicsInterface + * - Waitables: Supports NodeWaitablesInterface + * + * Or you can define your own interface support classes! + * + * Each of the support classes should define: + * - Default constructor + * - Templated constructor taking NodeT + * - get_node__interface() + * - set_node__interface() + * + * Usage example: + * - ```NodeInterfaces(node)``` + * will bind just the NodeBaseInterface. + * - ```NodeInterfaces< + * rclcpp::node_interfaces::Base, rclcpp::node_interfaces::Clock>(node)``` + * will bind both the NodeBaseInterface and NodeClockInterface. + * + * \param[in] node Node-like object to bind the interfaces of. + */ + template + NodeInterfaces(NodeT & node) // NOLINT(runtime/explicit) + : InterfaceTs(node)... {} // Implicit constructor for node-like passing to functions + + /// SharedPtr Constructor + template + NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) + : InterfaceTs(node ? *node : throw std::runtime_error("Passed in NodeT is nullptr!"))... {} +}; + + +/// Create a new NodeInterfaces object bound with no node interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * This method will return a NodeInterfaces with no bound interfaces. You must set them using + * ```NodeInterfaces->set__interface(InterfaceT::SharedPtr interface)``` + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr supporting the stated interfaces, but bound with none + * of them + */ +template +typename NodeInterfaces::SharedPtr +get_node_interfaces() +{ + static_assert(0 != sizeof ...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(); +} + +/// Create a new NodeInterfaces object bound with the passed in node-like object's interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr bound with the node-like objects's interfaces + */ +template +typename NodeInterfaces::SharedPtr +get_node_interfaces(NodeT & node) +{ + static_assert(0 != sizeof ...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(node); +} + +/// Create a new NodeInterfaces object bound with the passed in node-like shared_ptr's interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr bound with the node-like objects's interfaces + */ +template +typename NodeInterfaces::SharedPtr +get_node_interfaces(std::shared_ptr node) +{ + static_assert(0 != sizeof ...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(node); +} + +} // namespace node_interfaces +} // namespace fuse_core + +#endif // FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HPP_ diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp new file mode 100644 index 000000000..614cb326e --- /dev/null +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp @@ -0,0 +1,365 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ +#define FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ + +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace fuse_core +{ +namespace node_interfaces +{ + +/// TODO(CH3): Remove this once https://github.com/ros2/rclcpp/pull/2041 is merged and released + +// Helper classes to be inherited by NodeInterfaces to support node interface aggregation +// via multiple inheritance. + +// These also provide a more terse way to configure the supported interfaces! + + +/// NodeInterfaces support for NodeBaseInterface +class Base +{ +public: + /// Default constructor with no bound NodeBaseInterface + RCLCPP_PUBLIC + Base() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeBaseInterface + template + explicit Base(NodeT & node) {impl_ = node.get_node_base_interface();} + + /// Return the bound NodeBaseInterface + inline + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return impl_; + } + + /// Set the bound NodeBaseInterface + inline + void set_node_base_interface(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeClockInterface +class Clock +{ +public: + /// Default constructor with no bound NodeClockInterface + RCLCPP_PUBLIC + Clock() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeClockInterface + template + explicit Clock(NodeT & node) {impl_ = node.get_node_clock_interface();} + + /// Return the bound NodeClockInterface + inline + rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface() + { + return impl_; + } + + /// Set the bound NodeClockInterface + inline + void set_node_clock_interface(rclcpp::node_interfaces::NodeClockInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeGraphInterface +class Graph +{ +public: + /// Default constructor with no bound NodeGraphInterface + RCLCPP_PUBLIC + Graph() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeGraphInterface + template + explicit Graph(NodeT & node) {impl_ = node.get_node_graph_interface();} + + /// Return the bound NodeGraphInterface + inline + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface() + { + return impl_; + } + + /// Set the bound NodeGraphInterface + inline + void set_node_graph_interface(rclcpp::node_interfaces::NodeGraphInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeLoggingInterface +class Logging +{ +public: + /// Default constructor with no bound NodeLoggingInterface + RCLCPP_PUBLIC + Logging() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeLoggingInterface + template + explicit Logging(NodeT & node) {impl_ = node.get_node_logging_interface();} + + /// Return the bound NodeLoggingInterface + inline + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface() + { + return impl_; + } + + /// Set the bound NodeLoggingInterface + inline + void + set_node_logging_interface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeParametersInterface +class Parameters +{ +public: + /// Default constructor with no bound NodeParametersInterface + RCLCPP_PUBLIC + Parameters() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeParametersInterface + template + explicit Parameters(NodeT & node) {impl_ = node.get_node_parameters_interface();} + + /// Return the bound NodeParametersInterface + inline + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() + { + return impl_; + } + + /// Set the bound NodeParametersInterface + inline + void + set_node_parameters_interface(rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeServicesInterface +class Services +{ +public: + /// Default constructor with no bound NodeServicesInterface + RCLCPP_PUBLIC + Services() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeServicesInterface + template + explicit Services(NodeT & node) {impl_ = node.get_node_services_interface();} + + /// Return the bound NodeServicesInterface + inline + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface() + { + return impl_; + } + + /// Set the bound NodeServicesInterface + inline + void + set_node_services_interface(rclcpp::node_interfaces::NodeServicesInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeTimeSourceInterface +class TimeSource +{ +public: + /// Default constructor with no bound NodeTimeSourceInterface + RCLCPP_PUBLIC + TimeSource() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeTimeSourceInterface + template + explicit TimeSource(NodeT & node) {impl_ = node.get_node_time_source_interface();} + + /// Return the bound NodeTimeSourceInterface + inline + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface() + { + return impl_; + } + + /// Set the bound NodeTimeSourceInterface + inline + void + set_node_time_source_interface(rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeTimersInterface +class Timers +{ +public: + /// Default constructor with no bound NodeTimersInterface + RCLCPP_PUBLIC + Timers() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeTimersInterface + template + explicit Timers(NodeT & node) {impl_ = node.get_node_timers_interface();} + + /// Return the bound NodeTimersInterface + inline + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface() + { + return impl_; + } + + /// Set the bound NodeTimersInterface + inline + void + set_node_timers_interface(rclcpp::node_interfaces::NodeTimersInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeTopicsInterface +class Topics +{ +public: + /// Default constructor with no bound NodeTopicsInterface + RCLCPP_PUBLIC + Topics() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeTopicsInterface + template + explicit Topics(NodeT & node) {impl_ = node.get_node_topics_interface();} + + /// Return the bound NodeTopicsInterface + inline + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface() + { + return impl_; + } + + /// Set the bound NodeTopicsInterface + inline + void + set_node_topics_interface(rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr impl_; +}; + + +/// NodeInterfaces support for NodeWaitablesInterface +class Waitables +{ +public: + /// Default constructor with no bound NodeWaitablesInterface + RCLCPP_PUBLIC + Waitables() {impl_ = nullptr;} + + /// Bind the passed in node-like object's NodeWaitablesInterface + template + explicit Waitables(NodeT & node) {impl_ = node.get_node_waitables_interface();} + + /// Return the bound NodeWaitablesInterface + inline + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface() + { + return impl_; + } + + /// Set the bound NodeWaitablesInterface + inline + void + set_node_waitables_interface(rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr impl_; +}; + + +} // namespace node_interfaces +} // namespace fuse_core + +#endif // FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h index e0035ef67..2b44e6429 100644 --- a/fuse_core/include/fuse_core/parameter.h +++ b/fuse_core/include/fuse_core/parameter.h @@ -36,8 +36,10 @@ #include #include +#include -#include +#include +#include "rclcpp/parameter.hpp" #include #include @@ -46,21 +48,112 @@ namespace fuse_core { +// NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces + +/** + * @brief Compatibility wrapper for ros2 params in ros1 syntax + * + * Declare a parameter if not declared, otherwise, get its value. + * + * This is needed because the node parameters interface does not do the type conversions to and + * from ParameterValue for us. + * + * @param[in] interfaces - The node interfaces used to load the parameter + * @param[in] parameter_name - The ROS parameter name + * @param[out] default_value - The default value for this parameter + * @param[in] parameter_descriptor - An optional, custom description for the parameter. + * @param[in] When `true`, the parameter override is ignored. Default to `false`. + * \return The value of the parameter. + * @throws if the parameter has already been declared + */ +template +T getParam( + node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, + const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) +{ + auto params_interface = interfaces.get_node_parameters_interface(); + if (params_interface->has_parameter(parameter_name)) { + return params_interface->get_parameter(parameter_name).get_parameter_value().get(); + } else { + try { + return params_interface->declare_parameter( + parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, ignore_override + ).get(); + } catch (const rclcpp::ParameterTypeException & ex) { + throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); + } + } +} + + +/** + * @brief Compatibility wrapper for ros2 params in ros1 syntax + * + * Declare a parameter if not declared, otherwise, get its value. + * + * This is needed because the node parameters interface does not do the type conversions to and + * from ParameterValue for us. + * + * @param[in] interfaces - The node interfaces used to load the parameter + * @param[in] parameter_name - The ROS parameter name + * @param[in] parameter_descriptor - An optional, custom description for the parameter. + * @param[in] When `true`, the parameter override is ignored. Default to `false`. + * \return The value of the parameter. + * @throws if the parameter has already been declared + */ +template +T getParam( + node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) +{ + // get advantage of parameter value template magic to get + // the correct rclcpp::ParameterType from T + // NOTE(CH3): For the same reason we can't defer to the overload of getParam + rclcpp::ParameterValue value{T{}}; + try { + return interfaces.get_node_parameters_interface()->declare_parameter( + parameter_name, value.get_type(), parameter_descriptor, ignore_override + ).get(); + } catch (const rclcpp::ParameterTypeException & ex) { + throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); + } +} + /** * @brief Utility method for handling required ROS params * - * @param[in] nh - The ROS node handle with which to load parameters + * @param[in] interfaces - The node interfaces used to load the parameter * @param[in] key - The ROS parameter key for the required parameter * @param[out] value - The ROS parameter value for the \p key * @throws std::runtime_error if the parameter does not exist */ -template -void getParamRequired(const ros::NodeHandle& nh, const std::string& key, T& value) // TODO(CH3): Replace nh with a node -{ - if (!nh.getParam(key, value)) +inline +void getParamRequired( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& key, + std::string& value +){ + std::string default_value = ""; + value = getParam(interfaces, key, default_value); + + if (value == default_value) { - const std::string error = "Could not find required parameter " + key + " in namespace " + nh.getNamespace(); - RCLCPP_FATAL_STREAM(node->get_logger(), error); + const std::string error = + "Could not find required parameter " + key + " in namespace " + + interfaces.get_node_base_interface()->get_namespace(); + + RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), error); throw std::runtime_error(error); } } @@ -68,7 +161,7 @@ void getParamRequired(const ros::NodeHandle& nh, const std::string& key, T& valu /** * @brief Helper function that loads positive integral or floating point values from the parameter server * - * @param[in] node_handle - The node handle used to load the parameter + * @param[in] interfaces - The node interfaces used to load the parameter * @param[in] parameter_name - The parameter name to load * @param[in, out] default_value - A default value to use if the provided parameter name does not exist. As output it * has the loaded (or default) value @@ -76,15 +169,20 @@ void getParamRequired(const ros::NodeHandle& nh, const std::string& key, T& valu */ template ::value || std::is_floating_point::value>> -void getPositiveParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, T& default_value, - const bool strict = true) // TODO(CH3): Replace nh with a node -{ - T value; - node_handle.param(parameter_name, value, default_value); +void getPositiveParam( + node_interfaces::NodeInterfaces< + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& parameter_name, + T& default_value, + const bool strict = true +){ + T value = getParam(interfaces, parameter_name, default_value); if (value < 0 || (strict && value == 0)) { - RCLCPP_WARN_STREAM(node->get_logger(), - "The requested " << parameter_name << " is <" << (strict ? "=" : "") + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "") << " 0. Using the default value (" << default_value << ") instead."); } else @@ -102,12 +200,17 @@ void getPositiveParam(const ros::NodeHandle& node_handle, const std::string& par * has the loaded (or default) value * @param[in] strict - Whether to check the loaded value is strictly positive or not, i.e. whether 0 is accepted or not */ -inline void getPositiveParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, - rclcpp::Duration& default_value, const bool strict = true) +inline void getPositiveParam( + node_interfaces::NodeInterfaces< + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& parameter_name, + rclcpp::Duration& default_value, const bool strict = true) { double default_value_sec = default_value.seconds(); - getPositiveParam(node_handle, parameter_name, default_value_sec, strict); - default_value.fromSec(default_value_sec); + getPositiveParam(interfaces, parameter_name, default_value_sec, strict); + default_value = rclcpp::Duration::from_seconds(default_value_sec); } /** @@ -117,21 +220,25 @@ inline void getPositiveParam(const ros::NodeHandle& node_handle, const std::stri * @tparam Scalar - A scalar type, defaults to double * @tparam Size - An int size that specifies the expected size of the covariance matrix (rows and columns) * - * @param[in] node_handle - The node handle used to load the parameter + * @param[in] interfaces - The node interfaces used to load the parameter * @param[in] parameter_name - The parameter name to load * @param[in] default_value - A default value to use for all the diagonal elements if the provided parameter name does * not exist * @return The loaded (or default) covariance matrix, generated from the diagonal vector */ template -fuse_core::Matrix getCovarianceDiagonalParam(const ros::NodeHandle& node_handle, - const std::string& parameter_name, - Scalar default_value) -{ +fuse_core::Matrix getCovarianceDiagonalParam( + node_interfaces::NodeInterfaces< + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& parameter_name, + Scalar default_value +){ using Vector = typename Eigen::Matrix; std::vector diagonal(Size, default_value); - node_handle.param(parameter_name, diagonal, diagonal); + diagonal = getParam(interfaces, parameter_name, diagonal); const auto diagonal_size = diagonal.size(); if (diagonal_size != Size) @@ -152,22 +259,23 @@ fuse_core::Matrix getCovarianceDiagonalParam(const ros::Node /** * @brief Utility method to load a loss configuration * - * @param[in] nh - The ROS node handle with which to load parameters + * @param[in] interfaces - The node interfaces used to load the parameter * @param[in] name - The ROS parameter name for the loss configuration parameter * @return Loss function or nullptr if the parameter does not exist */ -inline fuse_core::Loss::SharedPtr loadLossConfig(const ros::NodeHandle& nh, const std::string& name) -{ - if (!nh.hasParam(name)) - { - return {}; - } - +inline fuse_core::Loss::SharedPtr loadLossConfig( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + const std::string& name +){ std::string loss_type; - getParamRequired(nh, name + "/type", loss_type); + getParamRequired(interfaces, name + "/type", loss_type); auto loss = fuse_core::createUniqueLoss(loss_type); - loss->initialize(nh.resolveName(name)); + loss->initialize(interfaces.get_node_base_interface()->get_fully_qualified_name()); return loss; } diff --git a/fuse_core/include/fuse_core/throttled_callback.h b/fuse_core/include/fuse_core/throttled_callback.h index 13f9032ec..661c26fa0 100644 --- a/fuse_core/include/fuse_core/throttled_callback.h +++ b/fuse_core/include/fuse_core/throttled_callback.h @@ -37,6 +37,9 @@ #include #include +#include +#include +#include namespace fuse_core { @@ -52,29 +55,23 @@ template class ThrottledCallback { public: - // TODO(CH3): Keep the use of "use_wall_time", but pass in a clock as appropriate to use - // non-wall-time. - // - // TODO(CH3): Add getters into any class that could add ThrottledCallbacks to obtain their - // NodeClockInterface to then get the clock to pass into this! /** * @brief Constructor * * @param[in] keep_callback The callback to call when kept, i.e. not dropped. Defaults to nullptr * @param[in] drop_callback The callback to call when dropped because of the throttling. Defaults to nullptr * @param[in] throttle_period The throttling period duration in seconds. Defaults to 0.0, i.e. no throttling - * @param[in] use_wall_time Whether to use wall time or not. Defaults to false + * @param[in] clock The clock to throttle against. Defaults to using RCL_SYSTEM_TIME */ ThrottledCallback(Callback&& keep_callback = nullptr, // NOLINT(whitespace/operators) Callback&& drop_callback = nullptr, // NOLINT(whitespace/operators) const rclcpp::Duration& throttle_period = rclcpp::Duration(0,0), - const bool use_wall_time = false) + rclcpp::Clock::SharedPtr clock = std::make_shared()) : keep_callback_(keep_callback) , drop_callback_(drop_callback) , throttle_period_(throttle_period) - , use_wall_time_(use_wall_time) - { - } + , clock_(clock) + {} /** * @brief Throttle period getter @@ -87,13 +84,16 @@ class ThrottledCallback } /** - * @brief Use wall time flag getter + * @brief Set the clock to throttle against * - * @return True if using wall time, false otherwise + * @param[in] clock The clock to set */ - bool getUseWallTime() const + void setClock(rclcpp::Clock::SharedPtr clock) { - return use_wall_time_; + clock_ = clock; + + // Reset since we changed the clock + last_called_time_ = rclcpp::Time(); } /** @@ -106,17 +106,6 @@ class ThrottledCallback throttle_period_ = throttle_period; } - // TODO(CH3): REFACTOR THIS OUT!!!! Take a clock type param instead - /** - * @brief Use wall time flag setter - * - * @param[in] use_wall_time Whether to use rclcpp::WallTime or not - */ - void setUseWallTime(const bool use_wall_time) - { - use_wall_time_ = use_wall_time; - } - /** * @brief Keep callback setter * @@ -161,9 +150,11 @@ class ThrottledCallback // (a) This is the first call, i.e. the last called time is still invalid because it has not been set yet // (b) The throttle period is zero, so we should always keep the callbacks // (c) The elpased time between now and the last called time is greater than the throttle period - #warn "using a valid time value as a flag" - const rclcpp::Time now = use_wall_time_ ? rclcpp::Clock::Clock(RCL_SYSTEM_TIME).now() : node->now(); - if ((last_called_time_.nanoseconds() == 0) || (throttle_period_.nanoseconds() == 0) || now - last_called_time_ > throttle_period_) + rclcpp::Time now = clock_->now(); + + if ((last_called_time_.nanoseconds() == 0) + || (throttle_period_.nanoseconds() == 0) + || now - last_called_time_ > throttle_period_) { if (keep_callback_) { @@ -200,8 +191,7 @@ class ThrottledCallback Callback keep_callback_; //!< The callback to call when kept, i.e. not dropped Callback drop_callback_; //!< The callback to call when dropped because of throttling rclcpp::Duration throttle_period_; //!< The throttling period duration in seconds - bool use_wall_time_; // -#include #include #include diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h index 2bac497fb..acb9004ad 100644 --- a/fuse_core/include/fuse_core/variable.h +++ b/fuse_core/include/fuse_core/variable.h @@ -42,9 +42,8 @@ #include #include -#include -#include #include +#include #include @@ -323,10 +322,10 @@ class Variable * * Defaults to -max. * - * @param[in] index The variable dimension of interest + * @param[in] index The variable dimension of interest (not used in base class) * @return The lower bound for the requested variable dimension */ - virtual double lowerBound(size_t index) const + virtual double lowerBound(size_t /* index */) const { return std::numeric_limits::lowest(); } @@ -336,10 +335,10 @@ class Variable * * Defaults to +max. * - * @param[in] index The variable dimension of interest + * @param[in] index The variable dimension of interest (not used in base class) * @return The upper bound for the requested variable dimension */ - virtual double upperBound(size_t index) const + virtual double upperBound(size_t /* index */) const { return std::numeric_limits::max(); } diff --git a/fuse_core/package.xml b/fuse_core/package.xml index ed03da956..65ad72e68 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -31,8 +31,11 @@ rcl_interfaces ament_cmake_gtest + ament_cmake_pytest ament_lint_auto ament_lint_common + geometry_msgs + launch_pytest ament_cmake diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index e744cb2bc..348a4e959 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -54,6 +54,14 @@ AsyncMotionModel::AsyncMotionModel(size_t thread_count) : { } +AsyncMotionModel::~AsyncMotionModel() +{ + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } +} + bool AsyncMotionModel::apply(Transaction& transaction) { // Insert a call to the motion model's queryCallback() function into the motion model's callback queue. While this @@ -97,6 +105,11 @@ void AsyncMotionModel::initialize(const std::string& name) onInit(); executor_->add_node(node_); + + // TODO(CH3): Remove this if the internal node is removed + spinner_ = std::thread([&](){ + executor_->spin(); + }); } void AsyncMotionModel::graphCallback(Graph::ConstSharedPtr graph) @@ -135,6 +148,12 @@ void AsyncMotionModel::stop() executor_->cancel(); executor_->remove_node(node_); + // TODO(CH3): Remove this if the internal node is removed + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + onStop(); } } diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index f038a6c1e..eb5a15011 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -45,6 +45,14 @@ AsyncPublisher::AsyncPublisher(size_t thread_count) : { } +AsyncPublisher::~AsyncPublisher() +{ + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } +} + void AsyncPublisher::initialize(const std::string& name) { // Initialize internal state @@ -71,6 +79,11 @@ void AsyncPublisher::initialize(const std::string& name) onInit(); executor_->add_node(node_); + + // TODO(CH3): Remove this if the internal node is removed + spinner_ = std::thread([&](){ + executor_->spin(); + }); } void AsyncPublisher::notify(Transaction::ConstSharedPtr transaction, Graph::ConstSharedPtr graph) @@ -104,6 +117,11 @@ void AsyncPublisher::stop() executor_->cancel(); executor_->remove_node(node_); + // TODO(CH3): Remove this if the internal node is removed + if (spinner_.joinable()) { + spinner_.join(); + } + onStop(); } } diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 5ccfcb3b1..4dd89f245 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -52,6 +52,13 @@ AsyncSensorModel::AsyncSensorModel(size_t thread_count) : { } +AsyncSensorModel::~AsyncSensorModel() +{ + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } +} void AsyncSensorModel::initialize( const std::string& name, @@ -84,6 +91,11 @@ void AsyncSensorModel::initialize( // Start the async spinner to service the local callback queue executor_->add_node(node_); + + // TODO(CH3): Remove this if the internal node is removed + spinner_ = std::thread([&](){ + executor_->spin(); + }); } void AsyncSensorModel::graphCallback(Graph::ConstSharedPtr graph) @@ -125,6 +137,11 @@ void AsyncSensorModel::stop() executor_->cancel(); executor_->remove_node(node_); + // TODO(CH3): Remove this if the internal node is removed + if (spinner_.joinable()) { + spinner_.join(); + } + onStop(); } } diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 99acbcc50..7195c8428 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include @@ -42,160 +42,647 @@ #include #include +// NOTE(CH3): Most of the parameter descriptions here were adapted from the parameter descriptions +// in the Ceres source code. +// +// https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/solver.h +// https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/covariance.h +// https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/problem.h +// ... namespace fuse_core { -void loadCovarianceOptionsFromROS(const ros::NodeHandle& nh, ceres::Covariance::Options& covariance_options) +// 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) { -#if CERES_VERSION_AT_LEAST(1, 13, 0) + return ns.empty() || ns.back() == '.' ? ns : ns + "."; +} + + +void loadCovarianceOptionsFromROS( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + ceres::Covariance::Options& covariance_options, + const std::string& namespace_string) +{ + rcl_interfaces::msg::ParameterDescriptor tmp_descr; + + std::string ns = get_well_formatted_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-solver/commit/14d8297cf968e421c5db4e3fb0543b3b111155d7 - covariance_options.sparse_linear_algebra_library_type = fuse_core::getParam( - nh, "sparse_linear_algebra_library_type", covariance_options.sparse_linear_algebra_library_type); -#endif - covariance_options.algorithm_type = fuse_core::getParam(nh, "algorithm_type", covariance_options.algorithm_type); - nh.param("min_reciprocal_condition_number", covariance_options.min_reciprocal_condition_number, - covariance_options.min_reciprocal_condition_number); - nh.param("null_space_rank", covariance_options.null_space_rank, covariance_options.null_space_rank); - nh.param("num_threads", covariance_options.num_threads, covariance_options.num_threads); - nh.param("apply_loss_function", covariance_options.apply_loss_function, covariance_options.apply_loss_function); + covariance_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( + interfaces, ns + "sparse_linear_algebra_library_type", + covariance_options.sparse_linear_algebra_library_type); + covariance_options.algorithm_type = + fuse_core::declareCeresParam(interfaces, ns + "algorithm_type", covariance_options.algorithm_type); + + tmp_descr.description = ( + "If DENSE_SVD is used, this parameter sets the threshold for determining if a Jacobian matrix " + "is rank deficient following the condition: " + "\n" + "min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)" + "\n" + "Where min_sigma and max_sigma are the minimum and maximum singular values of J respectively."); + covariance_options.min_reciprocal_condition_number = fuse_core::getParam( + interfaces, + ns + "min_reciprocal_condition_number", covariance_options.min_reciprocal_condition_number, tmp_descr + ); + + tmp_descr.description = + "The number of singular dimensions to tolerate (-1 unbounded) no effect on `SPARSE_QR`"; + covariance_options.null_space_rank = fuse_core::getParam( + interfaces, + ns + "null_space_rank", covariance_options.null_space_rank, tmp_descr + ); + + tmp_descr.description = + "Number of threads to be used for evaluating the Jacobian and estimation of covariance"; + covariance_options.num_threads = fuse_core::getParam( + interfaces, + ns + "num_threads", covariance_options.num_threads, tmp_descr + ); + + tmp_descr.description = ( + "false will turn off the application of the loss function to the output of the cost function " + "and in turn its effect on the covariance (does not affect residual blocks with built-in loss " + "functions)"); + covariance_options.apply_loss_function = fuse_core::getParam( + interfaces, + ns + "apply_loss_function", covariance_options.apply_loss_function, tmp_descr + ); } -void loadProblemOptionsFromROS(const ros::NodeHandle& nh, ceres::Problem::Options& problem_options) +void loadProblemOptionsFromROS( + node_interfaces::NodeInterfaces interfaces, + ceres::Problem::Options& problem_options, + const std::string& namespace_string) { - nh.param("enable_fast_removal", problem_options.enable_fast_removal, problem_options.enable_fast_removal); - nh.param("disable_all_safety_checks", problem_options.disable_all_safety_checks, - problem_options.disable_all_safety_checks); + rcl_interfaces::msg::ParameterDescriptor tmp_descr; + + std::string ns = get_well_formatted_namespace_string(namespace_string); + + tmp_descr.description = "trades memory for faster Problem::RemoveResidualBlock()"; + problem_options.enable_fast_removal = fuse_core::getParam( + interfaces, + ns + "enable_fast_removal", problem_options.enable_fast_removal, tmp_descr + ); + + tmp_descr.description = ( + "By default, Ceres performs a variety of safety checks when constructing " + "the problem. There is a small but measurable performance penalty to these " + "checks, typically around 5% of construction time. If you are sure your " + "problem construction is correct, and 5% of the problem construction time " + "is truly an overhead you want to avoid, then you can set " + "disable_all_safety_checks to true." + "\n" + "WARNING: Do not set this to true, unless you are absolutely sure of what " + "you are doing"); + problem_options.disable_all_safety_checks = fuse_core::getParam( + interfaces, + ns + "disable_all_safety_checks", problem_options.disable_all_safety_checks, tmp_descr + ); } -void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& solver_options) +void loadSolverOptionsFromROS( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Logging, + node_interfaces::Parameters + > interfaces, + ceres::Solver::Options& solver_options, + const std::string& namespace_string) { + rcl_interfaces::msg::ParameterDescriptor tmp_descr; + + std::string ns = get_well_formatted_namespace_string(namespace_string); + // Minimizer options - solver_options.minimizer_type = fuse_core::getParam(nh, "minimizer_type", solver_options.minimizer_type); - solver_options.line_search_direction_type = - fuse_core::getParam(nh, "line_search_direction_type", solver_options.line_search_direction_type); - solver_options.line_search_type = fuse_core::getParam(nh, "line_search_type", solver_options.line_search_type); - solver_options.nonlinear_conjugate_gradient_type = - fuse_core::getParam(nh, "nonlinear_conjugate_gradient_type", solver_options.nonlinear_conjugate_gradient_type); - - nh.param("max_lbfgs_rank", solver_options.max_lbfgs_rank, solver_options.max_lbfgs_rank); - nh.param("use_approximate_eigenvalue_bfgs_scaling", solver_options.use_approximate_eigenvalue_bfgs_scaling, - solver_options.use_approximate_eigenvalue_bfgs_scaling); - - solver_options.line_search_interpolation_type = - fuse_core::getParam(nh, "line_search_interpolation_type", solver_options.line_search_interpolation_type); - nh.param("min_line_search_step_size", solver_options.min_line_search_step_size, - solver_options.min_line_search_step_size); + solver_options.minimizer_type = + fuse_core::declareCeresParam(interfaces, ns + "minimizer_type", solver_options.minimizer_type); + solver_options.line_search_direction_type = fuse_core::declareCeresParam( + interfaces, ns + "line_search_direction_type", solver_options.line_search_direction_type); + solver_options.line_search_type = + fuse_core::declareCeresParam(interfaces, ns + "line_search_type", solver_options.line_search_type); + solver_options.nonlinear_conjugate_gradient_type = fuse_core::declareCeresParam( + interfaces, ns + "nonlinear_conjugate_gradient_type", + solver_options.nonlinear_conjugate_gradient_type); + + tmp_descr.description = ( + "The rank of the LBFGS hessian approximation. See: Nocedal, J. (1980). 'Updating Quasi-Newton " + "Matrices with Limited Storage'. Mathematics of Computation 35 (151): 773-782."); + solver_options.max_lbfgs_rank = fuse_core::getParam( + interfaces, + ns + "max_lbfgs_rank", + solver_options.max_lbfgs_rank, + tmp_descr + ); + + tmp_descr.description = ( + "As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), " + "the initial inverse Hessian approximation is taken to be the Identity. " + "However, Oren showed that using instead I * \\gamma, where \\gamma is " + "chosen to approximate an eigenvalue of the true inverse Hessian can " + "result in improved convergence in a wide variety of cases. Setting " + "use_approximate_eigenvalue_bfgs_scaling to true enables this scaling."); + solver_options.use_approximate_eigenvalue_bfgs_scaling = fuse_core::getParam( + interfaces, + ns + "use_approximate_eigenvalue_bfgs_scaling", + solver_options.use_approximate_eigenvalue_bfgs_scaling, + tmp_descr + ); + + tmp_descr.description = ( + "Degree of the polynomial used to approximate the objective function. Valid values are " + "BISECTION, QUADRATIC and CUBIC."); + solver_options.line_search_interpolation_type = fuse_core::declareCeresParam( + interfaces, + ns + "line_search_interpolation_type", + solver_options.line_search_interpolation_type); + + tmp_descr.description = + "If during the line search, the step_size falls below this value, it is truncated to zero."; + solver_options.min_line_search_step_size = fuse_core::getParam( + interfaces, + ns + "min_line_search_step_size", + solver_options.min_line_search_step_size, + tmp_descr + ); // Line search parameters - nh.param("line_search_sufficient_function_decrease", solver_options.line_search_sufficient_function_decrease, - solver_options.line_search_sufficient_function_decrease); - nh.param("max_line_search_step_contraction", solver_options.max_line_search_step_contraction, - solver_options.max_line_search_step_contraction); - nh.param("min_line_search_step_contraction", solver_options.min_line_search_step_contraction, - solver_options.min_line_search_step_contraction); - nh.param("max_num_line_search_step_size_iterations", solver_options.max_num_line_search_step_size_iterations, - solver_options.max_num_line_search_step_size_iterations); - nh.param("max_num_line_search_direction_restarts", solver_options.max_num_line_search_direction_restarts, - solver_options.max_num_line_search_direction_restarts); - nh.param("line_search_sufficient_curvature_decrease", solver_options.line_search_sufficient_curvature_decrease, - solver_options.line_search_sufficient_curvature_decrease); - nh.param("max_line_search_step_expansion", solver_options.max_line_search_step_expansion, - solver_options.max_line_search_step_expansion); - - solver_options.trust_region_strategy_type = - fuse_core::getParam(nh, "trust_region_strategy_type", solver_options.trust_region_strategy_type); - solver_options.dogleg_type = fuse_core::getParam(nh, "dogleg_type", solver_options.dogleg_type); - - nh.param("use_nonmonotonic_steps", solver_options.use_nonmonotonic_steps, solver_options.use_nonmonotonic_steps); - nh.param("max_consecutive_nonmonotonic_steps", solver_options.max_consecutive_nonmonotonic_steps, - solver_options.max_consecutive_nonmonotonic_steps); - - nh.param("max_num_iterations", solver_options.max_num_iterations, solver_options.max_num_iterations); - nh.param("max_solver_time_in_seconds", solver_options.max_solver_time_in_seconds, - solver_options.max_solver_time_in_seconds); - - nh.param("num_threads", solver_options.num_threads, solver_options.num_threads); - - nh.param("initial_trust_region_radius", solver_options.initial_trust_region_radius, - solver_options.initial_trust_region_radius); - nh.param("max_trust_region_radius", solver_options.max_trust_region_radius, solver_options.max_trust_region_radius); - nh.param("min_trust_region_radius", solver_options.min_trust_region_radius, solver_options.min_trust_region_radius); - - nh.param("min_relative_decrease", solver_options.min_relative_decrease, solver_options.min_relative_decrease); - nh.param("min_lm_diagonal", solver_options.min_lm_diagonal, solver_options.min_lm_diagonal); - nh.param("max_lm_diagonal", solver_options.max_lm_diagonal, solver_options.max_lm_diagonal); - nh.param("max_num_consecutive_invalid_steps", solver_options.max_num_consecutive_invalid_steps, - solver_options.max_num_consecutive_invalid_steps); - nh.param("function_tolerance", solver_options.function_tolerance, solver_options.function_tolerance); - nh.param("gradient_tolerance", solver_options.gradient_tolerance, solver_options.gradient_tolerance); - nh.param("parameter_tolerance", solver_options.parameter_tolerance, solver_options.parameter_tolerance); + tmp_descr.description = ( + "Solving the line search problem exactly is computationally " + "prohibitive. Fortunately, line search based optimization " + "algorithms can still guarantee convergence if instead of an " + "exact solution, the line search algorithm returns a solution " + "which decreases the value of the objective function " + "sufficiently. More precisely, we are looking for a step_size: " + "s.t. " + "f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size"); + solver_options.line_search_sufficient_function_decrease = fuse_core::getParam( + interfaces, + ns + "line_search_sufficient_function_decrease", + solver_options.line_search_sufficient_function_decrease, + tmp_descr + ); + + tmp_descr.description = ( + "In each iteration of the line search, " + "new_step_size >= max_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.max_line_search_step_contraction = fuse_core::getParam( + interfaces, + ns + "max_line_search_step_contraction", + solver_options.max_line_search_step_contraction, + tmp_descr + ); + + tmp_descr.description = ( + "In each iteration of the line search, " + "new_step_size <= min_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.min_line_search_step_contraction = fuse_core::getParam( + interfaces, + ns + "min_line_search_step_contraction", + solver_options.min_line_search_step_contraction, + tmp_descr + ); + + tmp_descr.description = ( + "Maximum number of trial step size iterations during each line " + "search, if a step size satisfying the search conditions cannot " + "be found within this number of trials, the line search will " + "terminate. " + + "The minimum allowed value is 0 for trust region minimizer and 1 " + "otherwise. If 0 is specified for the trust region minimizer, " + "then line search will not be used when solving constrained " + "optimization problems. "); + solver_options.max_num_line_search_step_size_iterations = fuse_core::getParam( + interfaces, + ns + "max_num_line_search_step_size_iterations", + solver_options.max_num_line_search_step_size_iterations, + tmp_descr + ); + + tmp_descr.description = ( + "Maximum number of restarts of the line search direction algorithm before " + "terminating the optimization. Restarts of the line search direction " + "algorithm occur when the current algorithm fails to produce a new descent " + "direction. This typically indicates a numerical failure, or a breakdown " + "in the validity of the approximations used. "); + solver_options.max_num_line_search_direction_restarts = fuse_core::getParam( + interfaces, + ns + "max_num_line_search_direction_restarts", + solver_options.max_num_line_search_direction_restarts, + tmp_descr + ); + + tmp_descr.description = ( + "The strong Wolfe conditions consist of the Armijo sufficient " + "decrease condition, and an additional requirement that the " + "step-size be chosen s.t. the _magnitude_ ('strong' Wolfe " + "conditions) of the gradient along the search direction " + "decreases sufficiently. Precisely, this second condition " + "is that we seek a step_size s.t. " + "\n" + " |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|" + "\n" + "Where f() is the line search objective and f'() is the derivative of f " + "w.r.t step_size (d f / d step_size)."); + solver_options.line_search_sufficient_curvature_decrease = fuse_core::getParam( + interfaces, + ns + "line_search_sufficient_curvature_decrease", + solver_options.line_search_sufficient_curvature_decrease, + tmp_descr + ); + + tmp_descr.description = ( + "During the bracketing phase of the Wolfe search, the step size is " + "increased until either a point satisfying the Wolfe conditions is " + "found, or an upper bound for a bracket containing a point satisfying " + "the conditions is found. Precisely, at each iteration of the expansion:" + "\n" + " new_step_size <= max_step_expansion * step_size." + "\n" + "By definition for expansion, max_step_expansion > 1.0."); + solver_options.max_line_search_step_expansion = fuse_core::getParam( + interfaces, + ns + "max_line_search_step_expansion", + solver_options.max_line_search_step_expansion, + tmp_descr + ); + + solver_options.trust_region_strategy_type = fuse_core::declareCeresParam( + interfaces, ns + "trust_region_strategy_type", solver_options.trust_region_strategy_type); + solver_options.dogleg_type = fuse_core::declareCeresParam( + interfaces, ns + "dogleg_type", solver_options.dogleg_type); + + + tmp_descr.description = ( + "Enables the non-monotonic trust region algorithm as described by Conn, " + "Gould & Toint in 'Trust Region Methods', Section 10.1"); + solver_options.use_nonmonotonic_steps = fuse_core::getParam( + interfaces, + ns + "use_nonmonotonic_steps", + solver_options.use_nonmonotonic_steps, + tmp_descr + ); + + tmp_descr.description = "The window size used by the step selection algorithm to accept non-monotonic steps"; + solver_options.max_consecutive_nonmonotonic_steps = fuse_core::getParam( + interfaces, + ns + "max_consecutive_nonmonotonic_steps", + solver_options.max_consecutive_nonmonotonic_steps, + tmp_descr + ); + + + tmp_descr.description = "Maximum number of iterations for which the solver should run"; + solver_options.max_num_iterations = fuse_core::getParam( + interfaces, + ns + "max_num_iterations", + solver_options.max_num_iterations, + tmp_descr + ); + + tmp_descr.description = "Maximum amount of time for which the solver should run"; + solver_options.max_solver_time_in_seconds = fuse_core::getParam( + interfaces, + ns + "max_solver_time_in_seconds", + solver_options.max_solver_time_in_seconds, + tmp_descr + ); + + tmp_descr.description = "Number of threads used by Ceres for evaluating the cost and jacobians"; + solver_options.num_threads = fuse_core::getParam( + interfaces, + ns + "num_threads", + solver_options.num_threads, + tmp_descr + ); + + tmp_descr.description = ( + "The size of the initial trust region. When the LEVENBERG_MARQUARDT strategy is used, the " + "reciprocal of this number is the initial regularization parameter"); + solver_options.initial_trust_region_radius = fuse_core::getParam( + interfaces, + ns + "initial_trust_region_radius", + solver_options.initial_trust_region_radius, + tmp_descr + ); + + tmp_descr.description = "The trust region radius is not allowed to grow beyond this value"; + solver_options.max_trust_region_radius = fuse_core::getParam( + interfaces, + ns + "max_trust_region_radius", + solver_options.max_trust_region_radius, + tmp_descr + ); + + tmp_descr.description = + "The solver terminates when the trust region becomes smaller than this value"; + solver_options.min_trust_region_radius = fuse_core::getParam( + interfaces, + ns + "min_trust_region_radius", + solver_options.min_trust_region_radius, + tmp_descr + ); + + tmp_descr.description = + "Lower threshold for relative decrease before a trust-region step is accepted"; + solver_options.min_relative_decrease = fuse_core::getParam( + interfaces, + ns + "min_relative_decrease", + solver_options.min_relative_decrease, + tmp_descr + ); + + tmp_descr.description = ( + "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the lower bound on the values of this diagonal matrix"); + solver_options.min_lm_diagonal = fuse_core::getParam( + interfaces, + ns + "min_lm_diagonal", + solver_options.min_lm_diagonal, + tmp_descr + ); + + tmp_descr.description = ( + "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the upper bound on the values of this diagonal matrix"); + solver_options.max_lm_diagonal = fuse_core::getParam( + interfaces, + ns + "max_lm_diagonal", + solver_options.max_lm_diagonal, + tmp_descr + ); + + tmp_descr.description = ( + "The step returned by a trust region strategy can sometimes be numerically invalid, usually " + "because of conditioning issues. Instead of crashing or stopping the optimization, the " + "optimizer can go ahead and try solving with a smaller trust region/better conditioned problem." + " This parameter sets the number of consecutive retries before the minimizer gives up"); + solver_options.max_num_consecutive_invalid_steps = fuse_core::getParam( + interfaces, + ns + "max_num_consecutive_invalid_steps", + solver_options.max_num_consecutive_invalid_steps, + tmp_descr + ); + + tmp_descr.description = + "Minimizer terminates when: (new_cost - old_cost) < function_tolerance * old_cost;"; + solver_options.function_tolerance = fuse_core::getParam( + interfaces, + ns + "function_tolerance", + solver_options.function_tolerance, + tmp_descr + ); + + tmp_descr.description = ( + "Minimizer terminates when: max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance" + "\n" + "This value should typically be 1e-4 * function_tolerance"); + solver_options.gradient_tolerance = fuse_core::getParam( + interfaces, + ns + "gradient_tolerance", + solver_options.gradient_tolerance, + tmp_descr + ); + + tmp_descr.description = + "Minimizer terminates when: |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)"; + solver_options.parameter_tolerance = fuse_core::getParam( + interfaces, + ns + "parameter_tolerance", + solver_options.parameter_tolerance, + tmp_descr + ); solver_options.linear_solver_type = - fuse_core::getParam(nh, "linear_solver_type", solver_options.linear_solver_type); + fuse_core::declareCeresParam(interfaces, ns + "linear_solver_type", solver_options.linear_solver_type); solver_options.preconditioner_type = - fuse_core::getParam(nh, "preconditioner_type", solver_options.preconditioner_type); + fuse_core::declareCeresParam(interfaces, ns + "preconditioner_type", solver_options.preconditioner_type); solver_options.visibility_clustering_type = - fuse_core::getParam(nh, "visibility_clustering_type", solver_options.visibility_clustering_type); + fuse_core::declareCeresParam(interfaces, ns + "visibility_clustering_type", solver_options.visibility_clustering_type); solver_options.dense_linear_algebra_library_type = - fuse_core::getParam(nh, "dense_linear_algebra_library_type", solver_options.dense_linear_algebra_library_type); - solver_options.sparse_linear_algebra_library_type = fuse_core::getParam( - nh, "sparse_linear_algebra_library_type", solver_options.sparse_linear_algebra_library_type); + fuse_core::declareCeresParam(interfaces, ns + "dense_linear_algebra_library_type", solver_options.dense_linear_algebra_library_type); + solver_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( + interfaces, ns + "sparse_linear_algebra_library_type", solver_options.sparse_linear_algebra_library_type); // No parameter is loaded for: std::shared_ptr linear_solver_ordering; - nh.param("use_explicit_schur_complement", solver_options.use_explicit_schur_complement, - solver_options.use_explicit_schur_complement); - nh.param("use_postordering", solver_options.use_postordering, solver_options.use_postordering); - nh.param("dynamic_sparsity", solver_options.dynamic_sparsity, solver_options.dynamic_sparsity); + + tmp_descr.description = + "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; + solver_options.use_explicit_schur_complement = fuse_core::getParam( + interfaces, + ns + "use_explicit_schur_complement", + solver_options.use_explicit_schur_complement, + tmp_descr + ); + + // Solved::Options::use_postordering was removed in: + // https://github.com/ceres-solver/ceres-solver/commit/8ba8fbb173db5a1e01feeafe875c1f04839fd97b + + tmp_descr.description = "This settings only affects the SPARSE_NORMAL_CHOLESKY solver."; + solver_options.dynamic_sparsity = fuse_core::getParam( + interfaces, + ns + "dynamic_sparsity", + solver_options.dynamic_sparsity, + tmp_descr + ); #if CERES_VERSION_AT_LEAST(2, 0, 0) - nh.param("use_mixed_precision_solves", solver_options.use_mixed_precision_solves, - solver_options.use_mixed_precision_solves); - nh.param("max_num_refinement_iterations", solver_options.max_num_refinement_iterations, - solver_options.max_num_refinement_iterations); + + tmp_descr.description = ( + "NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " + "\n" + "If use_mixed_precision_solves is true, the Gauss-Newton matrix " + "is computed in double precision, but its factorization is " + "computed in single precision. This can result in significant " + "time and memory savings at the cost of some accuracy in the " + "Gauss-Newton step. Iterative refinement is used to recover some " + "of this accuracy back."); + solver_options.use_mixed_precision_solves = fuse_core::getParam( + interfaces, + ns + "use_mixed_precision_solves", + solver_options.use_mixed_precision_solves, + tmp_descr + ); + + tmp_descr.description = + "Number steps of the iterative refinement process to run when computing the Gauss-Newton step."; + solver_options.max_num_refinement_iterations = fuse_core::getParam( + interfaces, + ns + "max_num_refinement_iterations", + solver_options.max_num_refinement_iterations, + tmp_descr + ); #endif - nh.param("use_inner_iterations", solver_options.use_inner_iterations, solver_options.use_inner_iterations); + + tmp_descr.description = + "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; + solver_options.use_inner_iterations = fuse_core::getParam( + interfaces, + ns + "use_inner_iterations", + solver_options.use_inner_iterations, + tmp_descr + ); // No parameter is loaded for: std::shared_ptr inner_iteration_ordering; - nh.param("inner_iteration_tolerance", solver_options.inner_iteration_tolerance, - solver_options.inner_iteration_tolerance); - nh.param("min_linear_solver_iterations", solver_options.min_linear_solver_iterations, - solver_options.min_linear_solver_iterations); - nh.param("max_linear_solver_iterations", solver_options.max_linear_solver_iterations, - solver_options.max_linear_solver_iterations); - nh.param("eta", solver_options.eta, solver_options.eta); - nh.param("jacobi_scaling", solver_options.jacobi_scaling, solver_options.jacobi_scaling); + tmp_descr.description = ( + "Once the relative decrease in the objective function due to " + "inner iterations drops below inner_iteration_tolerance, the use " + "of inner iterations in subsequent trust region minimizer " + "iterations is disabled."); + solver_options.inner_iteration_tolerance = fuse_core::getParam( + interfaces, + ns + "inner_iteration_tolerance", + solver_options.inner_iteration_tolerance, + tmp_descr + ); + + tmp_descr.description = "Minimum number of iterations used by the linear iterative solver"; + solver_options.min_linear_solver_iterations = fuse_core::getParam( + interfaces, + ns + "min_linear_solver_iterations", + solver_options.min_linear_solver_iterations, + tmp_descr + ); + + tmp_descr.description = "Maximum number of iterations used by the linear iterative solver"; + solver_options.max_linear_solver_iterations = fuse_core::getParam( + interfaces, + ns + "max_linear_solver_iterations", + solver_options.max_linear_solver_iterations, + tmp_descr + ); + + tmp_descr.description = ( + "Forcing sequence parameter. The truncated Newton solver uses this number to control the " + "relative accuracy with which the Newton step is computed"); + solver_options.eta = fuse_core::getParam( + interfaces, + ns + "eta", + solver_options.eta, + tmp_descr + ); + + + tmp_descr.description = ( + "True means that the Jacobian is scaled by the norm of its columns before being passed to the " + "linear solver. This improves the numerical conditioning of the normal equations"); + solver_options.jacobi_scaling = fuse_core::getParam( + interfaces, + ns + "jacobi_scaling", + solver_options.jacobi_scaling, + tmp_descr + ); // Logging options - solver_options.logging_type = fuse_core::getParam(nh, "logging_type", solver_options.logging_type); - nh.param("minimizer_progress_to_stdout", solver_options.minimizer_progress_to_stdout, - solver_options.minimizer_progress_to_stdout); - nh.param("trust_region_minimizer_iterations_to_dump", solver_options.trust_region_minimizer_iterations_to_dump, - solver_options.trust_region_minimizer_iterations_to_dump); - nh.param("trust_region_problem_dump_directory", solver_options.trust_region_problem_dump_directory, - solver_options.trust_region_problem_dump_directory); - solver_options.trust_region_problem_dump_format_type = fuse_core::getParam( - nh, "trust_region_problem_dump_format_type", solver_options.trust_region_problem_dump_format_type); + solver_options.logging_type = + fuse_core::declareCeresParam(interfaces, ns + "logging_type", solver_options.logging_type); + + tmp_descr.description = "If logging_type is not SILENT, sends the logging output to STDOUT"; + solver_options.minimizer_progress_to_stdout = fuse_core::getParam( + interfaces, + ns + "minimizer_progress_to_stdout", + solver_options.minimizer_progress_to_stdout, + tmp_descr + ); + fuse_core::getParam>( + interfaces, ns + "trust_region_minimizer_iterations_to_dump"); + std::vector iterations_to_dump_tmp = interfaces.get_node_parameters_interface() + ->get_parameter("trust_region_minimizer_iterations_to_dump") + .get_value>(); + if (!iterations_to_dump_tmp.empty()) { + solver_options.trust_region_minimizer_iterations_to_dump.reserve(iterations_to_dump_tmp.size()); + std::transform( + iterations_to_dump_tmp.begin(), + iterations_to_dump_tmp.end(), + std::back_inserter(solver_options.trust_region_minimizer_iterations_to_dump), + [](int64_t val){ return val; }); + } + + tmp_descr.description = ( + "Directory to which the problems should be written to. Should be " + "non-empty if trust_region_minimizer_iterations_to_dump is " + "non-empty and trust_region_problem_dump_format_type is not " + "CONSOLE."); + solver_options.trust_region_problem_dump_directory = fuse_core::getParam( + interfaces, + ns + "trust_region_problem_dump_directory", + solver_options.trust_region_problem_dump_directory, + tmp_descr + ); + solver_options.trust_region_problem_dump_format_type = + fuse_core::declareCeresParam( + interfaces, + ns + "trust_region_problem_dump_format_type", + solver_options.trust_region_problem_dump_format_type + ); // Finite differences options - nh.param("check_gradients", solver_options.check_gradients, solver_options.check_gradients); - nh.param("gradient_check_relative_precision", solver_options.gradient_check_relative_precision, - solver_options.gradient_check_relative_precision); - nh.param("gradient_check_numeric_derivative_relative_step_size", - solver_options.gradient_check_numeric_derivative_relative_step_size, - solver_options.gradient_check_numeric_derivative_relative_step_size); - nh.param("update_state_every_iteration", solver_options.update_state_every_iteration, - solver_options.update_state_every_iteration); + tmp_descr.description = ( + "Check all Jacobians computed by each residual block with finite differences, abort if numeric " + "and analytic gradients differ substantially)"); + solver_options.check_gradients = fuse_core::getParam( + interfaces, + ns + "check_gradients", + solver_options.check_gradients, + tmp_descr + ); + tmp_descr.description = ( + "Precision to check for in the gradient checker. If the relative " + "difference between an element in a Jacobian exceeds this number, then the Jacobian for that " + "cost term is dumped"); + solver_options.gradient_check_relative_precision = fuse_core::getParam( + interfaces, + ns + "gradient_check_relative_precision", + solver_options.gradient_check_relative_precision, + tmp_descr + ); + tmp_descr.description = ( + "Relative shift used for taking numeric derivatives when " + "Solver::Options::check_gradients is true."); + solver_options.gradient_check_numeric_derivative_relative_step_size = fuse_core::getParam( + interfaces, + ns + "gradient_check_numeric_derivative_relative_step_size", + solver_options.gradient_check_numeric_derivative_relative_step_size, + tmp_descr + ); + tmp_descr.description = ( + "If update_state_every_iteration is true, then Ceres Solver will guarantee that at the end of " + "every iteration and before any user IterationCallback is called, the parameter blocks are " + "updated to the current best solution found by the solver. Thus the IterationCallback can " + "inspect the values of the parameter blocks for purposes of computation, visualization or " + "termination"); + solver_options.update_state_every_iteration = fuse_core::getParam( + interfaces, + ns + "update_state_every_iteration", + solver_options.update_state_every_iteration, + tmp_descr + ); std::string error; if (!solver_options.IsValid(&error)) { - throw std::invalid_argument("Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); + throw std::invalid_argument( + "Invalid solver options in parameter " + + std::string(interfaces.get_node_base_interface()->get_namespace()) + + ". Error: " + error); } } diff --git a/fuse_core/test/CMakeLists.txt b/fuse_core/test/CMakeLists.txt new file mode 100644 index 000000000..215bc9dc5 --- /dev/null +++ b/fuse_core/test/CMakeLists.txt @@ -0,0 +1,61 @@ +# CORE GTESTS ====================================================================================== +ament_add_gtest(test_constraint test_constraint.cpp) +target_link_libraries(test_constraint ${PROJECT_NAME}) + +ament_add_gtest(test_eigen test_eigen.cpp) +target_link_libraries(test_eigen ${PROJECT_NAME}) + +ament_add_gtest(test_local_parameterization test_local_parameterization.cpp) +target_link_libraries(test_local_parameterization ${PROJECT_NAME}) + +ament_add_gtest(test_loss test_loss.cpp) +target_link_libraries(test_loss ${PROJECT_NAME}) + +ament_add_gtest(test_message_buffer test_message_buffer.cpp) +target_link_libraries(test_message_buffer ${PROJECT_NAME}) + +ament_add_gtest(test_timestamp_manager test_timestamp_manager.cpp) +target_link_libraries(test_timestamp_manager ${PROJECT_NAME}) + +ament_add_gtest(test_transaction test_transaction.cpp) +target_link_libraries(test_transaction ${PROJECT_NAME}) + +ament_add_gtest(test_util test_util.cpp) +target_link_libraries(test_util ${PROJECT_NAME}) + +ament_add_gtest(test_uuid test_uuid.cpp) +target_link_libraries(test_uuid ${PROJECT_NAME}) + +ament_add_gtest(test_variable test_variable.cpp) +target_link_libraries(test_variable ${PROJECT_NAME}) + + +# ROS TESTS (NO LAUNCH) ============================================================================ +ament_add_gtest(test_async_motion_model test_async_motion_model.cpp) +target_link_libraries(test_async_motion_model ${PROJECT_NAME}) + +ament_add_gtest(test_async_publisher test_async_publisher.cpp) +target_link_libraries(test_async_publisher ${PROJECT_NAME}) + +ament_add_gtest(test_async_sensor_model test_async_sensor_model.cpp) +target_link_libraries(test_async_sensor_model ${PROJECT_NAME}) + +ament_add_gtest(test_callback_wrapper test_callback_wrapper.cpp) +target_link_libraries(test_callback_wrapper ${PROJECT_NAME}) + +find_package(geometry_msgs REQUIRED) +ament_add_gtest(test_throttled_callback test_throttled_callback.cpp) +target_link_libraries(test_throttled_callback ${PROJECT_NAME} ${geometry_msgs_TARGETS}) + + +# ROS TESTS (WITH LAUNCH) ========================================================================== +find_package(ament_cmake_pytest REQUIRED) + +ament_add_gtest_executable(test_parameters launch_tests/test_parameters.cpp) +target_link_libraries(test_parameters ${PROJECT_NAME}) + +ament_add_pytest_test(test_parameters_py "launch_tests/test_parameters.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +) + +configure_file("launch_tests/test_parameters.yaml" "launch_tests/test_parameters.yaml" COPYONLY) diff --git a/fuse_core/test/async_motion_model.test b/fuse_core/test/async_motion_model.test deleted file mode 100644 index b380dc5d0..000000000 --- a/fuse_core/test/async_motion_model.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_core/test/async_publisher.test b/fuse_core/test/async_publisher.test deleted file mode 100644 index a3690015e..000000000 --- a/fuse_core/test/async_publisher.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_core/test/async_sensor_model.test b/fuse_core/test/async_sensor_model.test deleted file mode 100644 index 81d751cd0..000000000 --- a/fuse_core/test/async_sensor_model.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_core/test/callback_wrapper.test b/fuse_core/test/callback_wrapper.test deleted file mode 100644 index 2da4723a1..000000000 --- a/fuse_core/test/callback_wrapper.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp new file mode 100644 index 000000000..a7ca3f9ae --- /dev/null +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -0,0 +1,242 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath 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 +#include + +class TestParameters : public ::testing::Test +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(TestParameters, getPositiveParam) +{ + // Load parameters enforcing they are positive: + const double default_value{1.0}; + + auto node = rclcpp::Node::make_shared("test_parameters_node"); + + // Load a positive parameter: + { + double parameter{default_value}; + fuse_core::getPositiveParam(node, "positive_parameter", parameter); + EXPECT_EQ(3.0, parameter); + } + + // Load a negative parameter: + { + double parameter{default_value}; + fuse_core::getPositiveParam(node, "negative_parameter", parameter); + EXPECT_EQ(default_value, parameter); + } + + // Load a zero parameter: + { + double parameter{default_value}; + fuse_core::getPositiveParam(node, "zero_parameter", parameter); + EXPECT_EQ(default_value, parameter); + } + + // Load a zero parameter allowing zero (not strict): + { + double parameter{default_value}; + fuse_core::getPositiveParam(node, "zero_parameter", parameter, false); + EXPECT_EQ(0.0, parameter); + } +} + +TEST_F(TestParameters, GetCovarianceDiagonalParam) +{ + // Build expected covariance matrix: + constexpr int Size = 3; + constexpr double variance = 1.0e-3; + constexpr double default_variance = 0.0; + + fuse_core::Matrix3d expected_covariance = fuse_core::Matrix3d::Identity(); + expected_covariance *= variance; + + fuse_core::Matrix3d default_covariance = fuse_core::Matrix3d::Identity(); + default_covariance *= default_variance; + + // Load covariance matrix diagonal from the parameter server: + auto node = rclcpp::Node::make_shared("test_parameters_node"); + + // A covariance diagonal with the expected size and valid should be the same as the expected one: + { + const std::string parameter_name{"covariance_diagonal"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + + try { + const auto covariance = + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance); + + EXPECT_EQ(Size, covariance.rows()); + EXPECT_EQ(Size, covariance.cols()); + + EXPECT_EQ( + expected_covariance.rows() * expected_covariance.cols(), + expected_covariance.cwiseEqual(covariance).count()) + << "Expected\n" << expected_covariance << "\nActual\n" << covariance; + } catch (const std::exception & ex) { + FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); + } + } + + // If the parameter does not exist we should get the default covariance: + { + const std::string parameter_name{"non_existent_parameter"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + + try { + const auto covariance = + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance); + + EXPECT_EQ(Size, covariance.rows()); + EXPECT_EQ(Size, covariance.cols()); + + EXPECT_EQ( + default_covariance.rows() * default_covariance.cols(), + default_covariance.cwiseEqual(covariance).count()) + << "Expected\n" << default_covariance << "\nActual\n" << covariance; + } catch (const std::exception & ex) { + FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); + } + } + + // A covariance diagonal with negative values should throw std::invalid_argument: + { + const std::string parameter_name{"covariance_diagonal_with_negative_values"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + + EXPECT_THROW( + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance), + std::invalid_argument); + } + + // A covariance diagonal with size 2, smaller than expected, should throw std::invalid_argument: + { + const std::string parameter_name{"covariance_diagonal_with_size_2"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + + EXPECT_THROW( + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance), + std::invalid_argument); + } + + // A covariance diagonal with size 4, larger than expected, should throw std::invalid_argument: + { + const std::string parameter_name{"covariance_diagonal_with_size_4"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + + EXPECT_THROW( + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance), + std::invalid_argument); + } + + + // A covariance diagonal with an invalid element should throw rclcpp::exceptions::InvalidParameterTypeException: + { + const std::string parameter_name{"covariance_diagonal_with_strings"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + EXPECT_THROW( + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); + + // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to + // instead get substituted with default covariance. But now with strongly typed + // params in ROS 2, it throws (which is better behavior), so the test has been + // updated accordingly. + // + // See: https://github.com/locusrobotics/fuse/pull/286#discussion_r1035302941 + } + + // A covariance diagonal with an invalid element should throw rclcpp::exceptions::InvalidParameterTypeException: + { + const std::string parameter_name{"covariance_diagonal_with_string"}; + + ASSERT_FALSE(node->has_parameter(parameter_name)); + EXPECT_THROW( + fuse_core::getCovarianceDiagonalParam(node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); + + // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to + // instead get substituted with default covariance. But now with strongly typed + // params in ROS 2, it throws (which is better behavior), so the test has been + // updated accordingly. + // + // See: https://github.com/locusrobotics/fuse/pull/286#discussion_r1035302941 + } +} + + +// NOTE(CH3): This main is required because the test is manually run by a launch test +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return ret; +} diff --git a/fuse_core/test/launch_tests/test_parameters.py b/fuse_core/test/launch_tests/test_parameters.py new file mode 100755 index 000000000..9690362f2 --- /dev/null +++ b/fuse_core/test/launch_tests/test_parameters.py @@ -0,0 +1,48 @@ +#! /usr/bin/env python3 + +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_pytest +from launch_pytest.actions import ReadyToTest +from launch_pytest.tools import process as process_tools + +import pytest + + +@pytest.fixture +def test_proc(): + test_root = '.' + + param_path = os.path.join(test_root, 'launch_tests', 'test_parameters.yaml') + test_path = os.path.join(test_root, 'test_parameters') + + cmd = [test_path, '--ros-args', '--params-file', param_path] + return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) + + +@launch_pytest.fixture +def generate_test_description(test_proc): + return LaunchDescription([test_proc, ReadyToTest()]) + + +@pytest.mark.launch(fixture=generate_test_description) +async def test_no_failed_gtests(test_proc, launch_context): + await process_tools.wait_for_exit(launch_context, test_proc, timeout=10) + assert test_proc.return_code == 0, "GTests failed" diff --git a/fuse_core/test/launch_tests/test_parameters.yaml b/fuse_core/test/launch_tests/test_parameters.yaml new file mode 100644 index 000000000..12b333053 --- /dev/null +++ b/fuse_core/test/launch_tests/test_parameters.yaml @@ -0,0 +1,28 @@ +test_parameters_node: + ros__parameters: + # Covariance diagonal with expected size (3) and valid: + covariance_diagonal: [1.0e-3, 1.0e-3, 1.0e-3] + + # Covariance diagonal with negative values: + covariance_diagonal_with_negative_values: [1.0e-3, 1.0e-3, -1.0e-3] + + # Covariance diagonal with size 2, smaller than expected (3): + covariance_diagonal_with_size_2: [1.0e-3, 1.0e-3] + + # Covariance diagonal with size 4, larger than expected (3): + covariance_diagonal_with_size_4: [1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3] + + # Covariance diagonal with invalid element type: + covariance_diagonal_with_strings: ["NaN", "NaN", "NaN"] + + # Covariance diagonal with invalid type: + covariance_diagonal_with_string: "NaN" + + # Positive parameter: + positive_parameter: 3.0 + + # Negative parameter: + negative_parameter: -3.0 + + # Zero parameter: + zero_parameter: 0.0 diff --git a/fuse_core/test/parameter.yaml b/fuse_core/test/parameter.yaml deleted file mode 100644 index dd5ee182b..000000000 --- a/fuse_core/test/parameter.yaml +++ /dev/null @@ -1,26 +0,0 @@ -# Covariance diagonal with expected size (3) and valid: -covariance_diagonal: [1.0e-3, 1.0e-3, 1.0e-3] - -# Covariance diagonal with negative values: -covariance_diagonal_with_negative_values: [1.0e-3, 1.0e-3, -1.0e-3] - -# Covariance diagonal with size 2, smaller than expected (3): -covariance_diagonal_with_size_2: [1.0e-3, 1.0e-3] - -# Covariance diagonal with size 4, larger than expected (3): -covariance_diagonal_with_size_4: [1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3] - -# Covariance diagonal with invalid element type: -covariance_diagonal_with_strings: ["NaN", "NaN", "NaN"] - -# Covariance diagonal with invalid type: -covariance_diagonal_with_string: "NaN" - -# Positive parameter: -positive_parameter: 3.0 - -# Negative parameter: -negative_parameter: -3.0 - -# Zero parameter: -zero_parameter: 0.0 diff --git a/fuse_core/test/test_async_motion_model.cpp b/fuse_core/test/test_async_motion_model.cpp index 4dd0275a7..6e5d61314 100644 --- a/fuse_core/test/test_async_motion_model.cpp +++ b/fuse_core/test/test_async_motion_model.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include @@ -53,14 +53,14 @@ class MyMotionModel : public fuse_core::AsyncMotionModel bool applyCallback(fuse_core::Transaction& /*transaction*/) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(1.0)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); transaction_received = true; return true; } void onGraphUpdate(fuse_core::Graph::ConstSharedPtr /*graph*/) override { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(1.0)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); graph_received = true; } @@ -69,19 +69,33 @@ class MyMotionModel : public fuse_core::AsyncMotionModel initialized = true; } - bool graph_received; - bool initialized; - bool transaction_received; + bool graph_received = false; + bool initialized = false; + bool transaction_received = false; }; -TEST(AsyncMotionModel, OnInit) +class TestAsyncMotionModel : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestAsyncMotionModel, OnInit) { MyMotionModel motion_model; motion_model.initialize("my_motion_model"); EXPECT_TRUE(motion_model.initialized); } -TEST(AsyncMotionModel, OnGraphUpdate) +TEST_F(TestAsyncMotionModel, OnGraphUpdate) { MyMotionModel motion_model; motion_model.initialize("my_motion_model"); @@ -93,15 +107,17 @@ TEST(AsyncMotionModel, OnGraphUpdate) fuse_core::Graph::ConstSharedPtr graph; // nullptr...which is fine because we do not actually use it motion_model.graphCallback(graph); EXPECT_FALSE(motion_model.graph_received); - rclcpp::Time wait_time_elapsed = rclcpp::Clock(RCL_SYSTEM_TIME).now() + rclcpp::Duration::from_seconds(10.0); - while (!motion_model.graph_received && rclcpp::Clock(RCL_SYSTEM_TIME).now() < wait_time_elapsed) + + auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); + rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10.0); + while (!motion_model.graph_received && clock.now() < wait_time_elapsed) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.1)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } EXPECT_TRUE(motion_model.graph_received); } -TEST(AsyncMotionModel, ApplyCallback) +TEST_F(TestAsyncMotionModel, ApplyCallback) { MyMotionModel motion_model; motion_model.initialize("my_motion_model"); @@ -110,22 +126,10 @@ TEST(AsyncMotionModel, ApplyCallback) // will then inject a call to applyCallback() into the motion model's callback queue. There is a time delay there, so // this call should block for *at least* 1.0 second. Once it returns, the "received_transaction" flag should be set. fuse_core::Transaction transaction; - rclcpp::Time before_apply = rclcpp::Clock(RCL_SYSTEM_TIME).now(); + auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); + rclcpp::Time before_apply = clock.now(); motion_model.apply(transaction); - rclcpp::Time after_apply = rclcpp::Clock(RCL_SYSTEM_TIME).now(); + rclcpp::Time after_apply = clock.now(); EXPECT_TRUE(motion_model.transaction_received); EXPECT_LE(rclcpp::Duration::from_seconds(1.0), after_apply - before_apply); } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_async_motion_model"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_core/test/test_async_publisher.cpp b/fuse_core/test/test_async_publisher.cpp index e6c5b06d6..1232f79e4 100644 --- a/fuse_core/test/test_async_publisher.cpp +++ b/fuse_core/test/test_async_publisher.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include @@ -58,7 +58,7 @@ class MyPublisher : public fuse_core::AsyncPublisher fuse_core::Transaction::ConstSharedPtr /*transaction*/, fuse_core::Graph::ConstSharedPtr /*graph*/) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(1.0)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); callback_processed = true; } @@ -71,14 +71,28 @@ class MyPublisher : public fuse_core::AsyncPublisher bool initialized; }; -TEST(AsyncPublisher, OnInit) +class TestAsyncPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestAsyncPublisher, OnInit) { MyPublisher publisher; publisher.initialize("my_publisher"); EXPECT_TRUE(publisher.initialized); } -TEST(AsyncPublisher, notifyCallback) +TEST_F(TestAsyncPublisher, notifyCallback) { MyPublisher publisher; publisher.initialize("my_publisher"); @@ -91,23 +105,13 @@ TEST(AsyncPublisher, notifyCallback) fuse_core::Graph::ConstSharedPtr graph; // nullptr...which is fine because we do not actually use it publisher.notify(transaction, graph); EXPECT_FALSE(publisher.callback_processed); - rclcpp::Time wait_time_elapsed = rclcpp::Clock(RCL_SYSTEM_TIME).now() + rclcpp::Duration::from_seconds(10.0); - while (!publisher.callback_processed && rclcpp::Clock(RCL_SYSTEM_TIME).now() < wait_time_elapsed) + + auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); + + rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10.0); + while (!publisher.callback_processed && clock.now() < wait_time_elapsed) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.1)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } EXPECT_TRUE(publisher.callback_processed); } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_async_publisher"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_core/test/test_async_sensor_model.cpp b/fuse_core/test/test_async_sensor_model.cpp index d6819fe10..22bbb14db 100644 --- a/fuse_core/test/test_async_sensor_model.cpp +++ b/fuse_core/test/test_async_sensor_model.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include @@ -47,7 +47,7 @@ static bool received_transaction = false; */ void transactionCallback(fuse_core::Transaction::SharedPtr /*transaction*/) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(1.0)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); received_transaction = true; } @@ -72,22 +72,36 @@ class MySensor : public fuse_core::AsyncSensorModel void onGraphUpdate(fuse_core::Graph::ConstSharedPtr /*graph*/) override { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(1.0)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); graph_received = true; } - bool graph_received; - bool initialized; + bool graph_received = false; + bool initialized = false; }; -TEST(AsyncSensorModel, OnInit) +class TestAsyncSensorModel : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestAsyncSensorModel, OnInit) { MySensor sensor; sensor.initialize("my_sensor", &transactionCallback); EXPECT_TRUE(sensor.initialized); } -TEST(AsyncSensorModel, OnGraphUpdate) +TEST_F(TestAsyncSensorModel, OnGraphUpdate) { MySensor sensor; sensor.initialize("my_sensor", &transactionCallback); @@ -99,15 +113,16 @@ TEST(AsyncSensorModel, OnGraphUpdate) fuse_core::Graph::ConstSharedPtr graph; // nullptr...which is fine because we do not actually use it sensor.graphCallback(graph); EXPECT_FALSE(sensor.graph_received); - rclcpp::Time wait_time_elapsed = rclcpp::Clock(RCL_SYSTEM_TIME).now() + rclcpp::Duration::from_seconds(10.0); - while (!sensor.graph_received && rclcpp::Clock(RCL_SYSTEM_TIME).now() < wait_time_elapsed) + auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); + rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10.0); + while (!sensor.graph_received && clock.now() < wait_time_elapsed) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.1)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } EXPECT_TRUE(sensor.graph_received); } -TEST(AsyncSensorModel, SendTransaction) +TEST_F(TestAsyncSensorModel, SendTransaction) { MySensor sensor; sensor.initialize("my_sensor", &transactionCallback); @@ -117,16 +132,3 @@ TEST(AsyncSensorModel, SendTransaction) sensor.sendTransaction(transaction); EXPECT_TRUE(received_transaction); } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_async_sensor_model"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_core/test/test_callback_wrapper.cpp b/fuse_core/test/test_callback_wrapper.cpp index 6656bfd08..4be0bedcd 100644 --- a/fuse_core/test/test_callback_wrapper.cpp +++ b/fuse_core/test/test_callback_wrapper.cpp @@ -32,13 +32,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include -#include +#include -#include #include #include +#include #include #include @@ -57,42 +56,63 @@ class MyClass } }; -TEST(CallbackWrapper, Double) +class TestCallbackWrapper : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestCallbackWrapper, Double) { MyClass my_object; std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; - auto callback = boost::make_shared>( + + auto node = rclcpp::Node::make_shared("callback_wrapper_double_test_node"); + auto callback_queue = + std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable( + callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + + auto callback = std::make_shared>( std::bind(&MyClass::processData, &my_object, std::ref(data))); auto result = callback->getFuture(); - ros::getGlobalCallbackQueue()->addCallback(callback); - result.wait(); + callback_queue->addCallback(callback); + rclcpp::spin_until_future_complete(node, result); + + // This is technically redundant but this is here to mimic how the callbacks are actually used + EXPECT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); EXPECT_EQ(15.0, result.get()); } -TEST(CallbackWrapper, Void) +TEST_F(TestCallbackWrapper, Void) { MyClass my_object; std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; double output; - auto callback = boost::make_shared>( + + auto node = rclcpp::Node::make_shared("callback_wrapper_void_test_node"); + auto callback_queue = + std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable( + callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + + auto callback = std::make_shared>( std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output))); auto result = callback->getFuture(); - ros::getGlobalCallbackQueue()->addCallback(callback); - result.wait(); - EXPECT_EQ(15.0, output); -} + callback_queue->addCallback(callback); + rclcpp::spin_until_future_complete(node, result); -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "callback_wrapper_test"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; + // This is technically redundant but this is here to mimic how the callbacks are actually used + EXPECT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_EQ(15.0, output); } diff --git a/fuse_core/test/test_constraint.cpp b/fuse_core/test/test_constraint.cpp index 7b453ce03..1599cca8b 100644 --- a/fuse_core/test/test_constraint.cpp +++ b/fuse_core/test/test_constraint.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include "example_constraint.h" #include @@ -96,9 +96,3 @@ TEST(Constraint, Type) ExampleConstraint constraint("test", {variable_uuid1}); // NOLINT ASSERT_EQ("ExampleConstraint", constraint.type()); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_eigen.cpp b/fuse_core/test/test_eigen.cpp index 6c76e5092..da4972ead 100644 --- a/fuse_core/test/test_eigen.cpp +++ b/fuse_core/test/test_eigen.cpp @@ -97,9 +97,3 @@ TEST(Eigen, isPositiveDefinite) // // EXPECT_FALSE(fuse_core::isPositiveDefinite(non_square_matrix)); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_local_parameterization.cpp b/fuse_core/test/test_local_parameterization.cpp index 2e2c2f3a7..1bda83662 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -125,9 +125,3 @@ TEST(LocalParameterization, MinusJacobian) EXPECT_TRUE(success); EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_loss.cpp b/fuse_core/test/test_loss.cpp index 8dd7ae606..ce5c72c59 100644 --- a/fuse_core/test/test_loss.cpp +++ b/fuse_core/test/test_loss.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "example_loss.h" #include @@ -51,9 +51,3 @@ TEST(Loss, Constructor) delete loss_function; } } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_message_buffer.cpp b/fuse_core/test/test_message_buffer.cpp index 9ccc65625..ef82327ec 100644 --- a/fuse_core/test/test_message_buffer.cpp +++ b/fuse_core/test/test_message_buffer.cpp @@ -200,9 +200,3 @@ TEST_F(MessageBufferTestFixture, Purge) EXPECT_EQ(rclcpp::Time(1000, 0), *stamps_range_iter); } } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_parameter.cpp b/fuse_core/test/test_parameter.cpp deleted file mode 100644 index e55056dd6..000000000 --- a/fuse_core/test/test_parameter.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Clearpath 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 -#include - -TEST(Parameter, GetPositiveParam) -{ - // Load parameters enforcing they are positive: - const double default_value{ 1.0 }; - - ros::NodeHandle node_handle; - - // Load a positive parameter: - { - double parameter{ default_value }; - fuse_core::getPositiveParam(node_handle, "positive_parameter", parameter); - EXPECT_EQ(3.0, parameter); - } - - // Load a negative parameter: - { - double parameter{ default_value }; - fuse_core::getPositiveParam(node_handle, "negative_parameter", parameter); - EXPECT_EQ(default_value, parameter); - } - - // Load a zero parameter: - { - double parameter{ default_value }; - fuse_core::getPositiveParam(node_handle, "zero_parameter", parameter); - EXPECT_EQ(default_value, parameter); - } - - // Load a zero parameter allowing zero (not strict): - { - double parameter{ default_value }; - fuse_core::getPositiveParam(node_handle, "zero_parameter", parameter, false); - EXPECT_EQ(0.0, parameter); - } -} - -TEST(Parameter, GetCovarianceDiagonalParam) -{ - // Build expected covariance matrix: - constexpr int Size = 3; - constexpr double variance = 1.0e-3; - constexpr double default_variance = 0.0; - - fuse_core::Matrix3d expected_covariance = fuse_core::Matrix3d::Identity(); - expected_covariance *= variance; - - fuse_core::Matrix3d default_covariance = fuse_core::Matrix3d::Identity(); - default_covariance *= default_variance; - - // Load covariance matrix diagonal from the parameter server: - ros::NodeHandle node_handle; - - // A covariance diagonal with the expected size and valid should be the same as the expected one: - { - const std::string parameter_name{ "covariance_diagonal" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - try - { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance); - - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); - - EXPECT_EQ(expected_covariance.rows() * expected_covariance.cols(), - expected_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << expected_covariance << "\nActual\n" << covariance; - } - catch (const std::exception& ex) - { - FAIL() << "Failed to get " << node_handle.resolveName(parameter_name) << ": " << ex.what(); - } - } - - // If the parameter does not exist we should get the default covariance: - { - const std::string parameter_name{ "non_existent_parameter" }; - - ASSERT_FALSE(node_handle.hasParam(parameter_name)); - - try - { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance); - - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); - - EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), - default_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << default_covariance << "\nActual\n" << covariance; - } - catch (const std::exception& ex) - { - FAIL() << "Failed to get " << node_handle.resolveName(parameter_name) << ": " << ex.what(); - } - } - - // A covariance diagonal with negative values should throw std::invalid_argument: - { - const std::string parameter_name{ "covariance_diagonal_with_negative_values" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance), - std::invalid_argument); - } - - // A covariance diagonal with size 2, smaller than expected, should throw std::invalid_argument: - { - const std::string parameter_name{ "covariance_diagonal_with_size_2" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance), - std::invalid_argument); - } - - // A covariance diagonal with size 4, larger than expected, should throw std::invalid_argument: - { - const std::string parameter_name{ "covariance_diagonal_with_size_4" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance), - std::invalid_argument); - } - - // A covariance diagonal with invalid element type does not throw, but nothing it is loaded, so we should get the - // default covariance: - { - const std::string parameter_name{ "covariance_diagonal_with_strings" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - try - { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance); - - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); - - EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), - default_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << default_covariance << "\nActual\n" << covariance; - } - catch (const std::exception& ex) - { - FAIL() << "Failed to get " << node_handle.resolveName(parameter_name) << ": " << ex.what(); - } - } - - // A covariance diagonal with invalid element type does not throw, but nothing it is loaded, so we should get the - // default covariance: - { - const std::string parameter_name{ "covariance_diagonal_with_string" }; - - ASSERT_TRUE(node_handle.hasParam(parameter_name)); - - try - { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(node_handle, parameter_name, default_variance); - - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); - - EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), - default_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << default_covariance << "\nActual\n" << covariance; - } - catch (const std::exception& ex) - { - FAIL() << "Failed to get " << node_handle.resolveName(parameter_name) << ": " << ex.what(); - } - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "parameter_test"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index 8a233381f..37f41b7ff 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -32,19 +32,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include -#include +#include +#include +#include #include - /** - * @brief A helper class to publish a given number geometry_msgs::Point messages at a given frequency. + * @brief A helper class to publish a given number geometry_msgs::msg::Point messages at a given frequency. * - * The messages published are geometry_msgs::Point because it is simple. The 'x' field is set to the number of messages + * The messages published are geometry_msgs::msg::Point because it is simple. The 'x' field is set to the number of messages * published so far, starting at 0. */ -class PointPublisher +class PointPublisher : public rclcpp::Node { public: /** @@ -53,9 +53,20 @@ class PointPublisher * @param[in] frequency The publishing frequency in Hz */ explicit PointPublisher(const double frequency) - : frequency_(frequency) + : Node("point_publisher_node") + , frequency_(frequency) { - publisher_ = node_handle_.advertise("point", 1); + publisher_ = this->create_publisher("point", 1); + } + + /** + * @brief Get the internal node pointer + * + * @return the node pointer + */ + rclcpp::Node::SharedPtr getNode() + { + return shared_from_this(); } /** @@ -65,43 +76,39 @@ class PointPublisher */ void publish(const size_t num_messages) { - // Wait for the subscribers to be ready before sending them data: - rclcpp::Time subscriber_timeout = this->node_->now() + rclcpp::Duration::from_seconds(1.0); - while (publisher_.getNumSubscribers() < 1u && this->node_->now() < subscriber_timeout) + // Wait for the subscriptions to be ready before sending them data: + rclcpp::Time subscription_timeout = this->now() + rclcpp::Duration::from_seconds(1.0); + while (publisher_->get_subscription_count() < 1u && this->now() < subscription_timeout) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.01); + rclcpp::sleep_for(std::chrono::milliseconds(10)); } - ASSERT_GE(publisher_.getNumSubscribers(), 1u); + ASSERT_GE(publisher_->get_subscription_count(), 1u); // Send data: - ros::Rate rate(frequency_); + rclcpp::Rate rate(frequency_); for (size_t i = 0; i < num_messages; ++i) { - geometry_msgs::Point point_message; + geometry_msgs::msg::Point point_message; point_message.x = i; - - publisher_.publish(point_message); - + publisher_->publish(point_message); rate.sleep(); } } private: - // TODO(CH3): Make this an rclcpp node. It's a test, we don't need the node interfaces. - ros::NodeHandle node_handle_; //!< The node handle - ros::Publisher publisher_; //!< The publisher + rclcpp::Publisher::SharedPtr publisher_; //!< The publisher double frequency_{ 10.0 }; //!< The publish rate frequency }; /** - * @brief A dummy point sensor model that uses a fuse_core::ThrottledMessageCallback with a keep + * @brief A dummy point sensor model that uses a fuse_core::ThrottledMessageCallback with a keep * and drop callback. * * The callbacks simply count the number of times they are called, for testing purposes. The keep callback also caches * the last message received, also for testing purposes. */ -class PointSensorModel +class PointSensorModel : public rclcpp::Node { public: /** @@ -110,11 +117,29 @@ class PointSensorModel * @param[in] throttle_period The throttle period duration in seconds */ explicit PointSensorModel(const rclcpp::Duration& throttle_period) - : throttled_callback_(std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), - std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), throttle_period) + : Node("point_sensor_model_node") + , throttled_callback_( + std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), + std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), + throttle_period + ) { - subscriber_ = node_handle_.subscribe( - "point", 10, &PointThrottledCallback::callback, &throttled_callback_); + subscription_ = this->create_subscription( + "point", 10, + std::bind( + &PointThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1) + ); + } + + /** + * @brief Get the internal node pointer + * + * @return the node pointer + */ + rclcpp::Node::SharedPtr getNode() + { + return shared_from_this(); } /** @@ -142,7 +167,7 @@ class PointSensorModel * * @return The last message kept. It would be nullptr if no message has been kept so far */ - const geometry_msgs::Point::ConstPtr getLastKeptMessage() const + const geometry_msgs::msg::Point::ConstSharedPtr getLastKeptMessage() const { return last_kept_message_; } @@ -151,9 +176,9 @@ class PointSensorModel /** * @brief Keep callback, that counts the number of times it has been called and caches the last message received * - * @param[in] msg A geometry_msgs::Point message + * @param[in] msg A geometry_msgs::msg::Point message */ - void keepCallback(const geometry_msgs::Point::ConstPtr& msg) + void keepCallback(const geometry_msgs::msg::Point::ConstSharedPtr& msg) { ++kept_messages_; last_kept_message_ = msg; @@ -162,56 +187,84 @@ class PointSensorModel /** * @brief Drop callback, that counts the number of times it has been called * - * @param[in] msg A geometry_msgs::Point message (not used) + * @param[in] msg A geometry_msgs::msg::Point message (not used) */ - void dropCallback(const geometry_msgs::Point::ConstPtr& /*msg*/) + // NOTE(CH3): The ConstSharedPtr here is necessary to allow binding the throttled callback + void dropCallback(const geometry_msgs::msg::Point::ConstSharedPtr& /*msg*/) { ++dropped_messages_; } - ros::NodeHandle node_handle_; //!< The node handle - ros::Subscriber subscriber_; //!< The subscriber + rclcpp::Subscription::SharedPtr subscription_; //!< The subscription - using PointThrottledCallback = fuse_core::ThrottledMessageCallback; + using PointThrottledCallback = fuse_core::ThrottledMessageCallback; PointThrottledCallback throttled_callback_; //!< The throttled callback size_t kept_messages_{ 0 }; //!< Messages kept size_t dropped_messages_{ 0 }; //!< Messages dropped - geometry_msgs::Point::ConstPtr last_kept_message_; //!< The last message kept + geometry_msgs::msg::Point::ConstSharedPtr last_kept_message_; //!< The last message kept }; - -TEST(ThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) +class TestThrottledCallback : public ::testing::Test { - // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify. - // TODO(CH3): Refactor this with a passed in node - // ASSERT_TRUE(fuse_core::wait_for_valid(->get_clock(), rclcpp::Duration::from_seconds(1.0))); +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread([&](){ + executor_->spin(); + }); + } + void TearDown() override + { + executor_->cancel(); + rclcpp::shutdown(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) +{ // Start sensor model to listen to messages: - const rclcpp::Duration throttled_period(0.0); - PointSensorModel sensor_model(throttled_period); + const rclcpp::Duration throttled_period(0, 0); + auto sensor_model = std::make_shared(throttled_period); + executor_->add_node(sensor_model); + + // Time should be valid after the context is initialized. But it doesn't hurt to verify. + ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(), + rclcpp::Duration::from_seconds(1.0))); // Publish some messages: const size_t num_messages = 10; const double frequency = 10.0; - PointPublisher publisher(frequency); - publisher.publish(num_messages); + auto publisher = std::make_shared(frequency); + executor_->add_node(publisher); + publisher->publish(num_messages); // Check all messages are kept and none are dropped, because when the throttle period is zero, throttling is disabled: - EXPECT_EQ(num_messages, sensor_model.getKeptMessages()); - EXPECT_EQ(0u, sensor_model.getDroppedMessages()); + EXPECT_EQ(num_messages, sensor_model->getKeptMessages()); + EXPECT_EQ(0u, sensor_model->getDroppedMessages()); } -TEST(ThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPeriod) +TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPeriod) { - // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify. - // TODO(CH3): Refactor this with a passed in node - // ASSERT_TRUE(fuse_core::wait_for_valid(->get_clock(), rclcpp::Duration::from_seconds(1.0))); - // Start sensor model to listen to messages: - const rclcpp::Duration throttled_period(0.2); - PointSensorModel sensor_model(throttled_period); + const rclcpp::Duration throttled_period(0, RCUTILS_S_TO_NS(0.2)); + auto sensor_model = std::make_shared(throttled_period); + executor_->add_node(sensor_model); + + // Time should be valid after the context is initialized. But it doesn't hurt to verify. + ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(), + rclcpp::Duration::from_seconds(1.0))); // Publish some messages at half the throttled period: const size_t num_messages = 10; @@ -219,55 +272,46 @@ TEST(ThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPeriod) const double period = period_factor * throttled_period.seconds(); const double frequency = 1.0 / period; - PointPublisher publisher(frequency); - publisher.publish(num_messages); + auto publisher = std::make_shared(frequency); + executor_->add_node(publisher); + publisher->publish(num_messages); // Check the number of kept and dropped callbacks: const auto expected_kept_messages = period_factor * num_messages; const auto expected_dropped_messages = num_messages - expected_kept_messages; - EXPECT_NEAR(expected_kept_messages, sensor_model.getKeptMessages(), 1.0); - EXPECT_NEAR(expected_dropped_messages, sensor_model.getDroppedMessages(), 1.0); + EXPECT_NEAR(expected_kept_messages, sensor_model->getKeptMessages(), 1.0); + EXPECT_NEAR(expected_dropped_messages, sensor_model->getDroppedMessages(), 1.0); } -TEST(ThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLarge) +TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLarge) { - // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify. - // TODO(CH3): Refactor this with a passed in node - // ASSERT_TRUE(fuse_core::wait_for_valid(->get_clock(), rclcpp::Duration::from_seconds(1.0))); - // Start sensor model to listen to messages: - const rclcpp::Duration throttled_period(10.0); - PointSensorModel sensor_model(throttled_period); + const rclcpp::Duration throttled_period(10, 0); + auto sensor_model = std::make_shared(throttled_period); + executor_->add_node(sensor_model); - ASSERT_EQ(nullptr, sensor_model.getLastKeptMessage()); + // Time should be valid after the context is initialized. But it doesn't hurt to verify. + ASSERT_TRUE(fuse_core::wait_for_valid(sensor_model->getNode()->get_clock(), + rclcpp::Duration::from_seconds(1.0))); + + ASSERT_EQ(nullptr, sensor_model->getLastKeptMessage()); // Publish some messages: const size_t num_messages = 10; const double period = 0.1 * num_messages / throttled_period.seconds(); const double frequency = 1.0 / period; - PointPublisher publisher(frequency); - publisher.publish(num_messages); + auto publisher = std::make_shared(frequency); + publisher->publish(num_messages); + executor_->add_node(publisher); // Check that regardless of the large throttled period, at least one message is ketpt: - EXPECT_EQ(1u, sensor_model.getKeptMessages()); - EXPECT_EQ(num_messages - 1u, sensor_model.getDroppedMessages()); + EXPECT_EQ(1u, sensor_model->getKeptMessages()); + EXPECT_EQ(num_messages - 1u, sensor_model->getDroppedMessages()); // Check the message kept was the first message: - const auto last_kept_message = sensor_model.getLastKeptMessage(); + const auto last_kept_message = sensor_model->getLastKeptMessage(); ASSERT_NE(nullptr, last_kept_message); EXPECT_EQ(0.0, last_kept_message->x); } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "throttled_callback_test"); - auto spinner = ros::AsyncSpinner(1); - spinner.start(); - int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); - return ret; -} diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index 062ef6793..e14644966 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -855,9 +855,3 @@ TEST_F(TimestampManagerTestFixture, SplitSameMultiple) EXPECT_EQ(rclcpp::Time(29, 0), generated_time_spans[4].first); EXPECT_EQ(rclcpp::Time(30, 0), generated_time_spans[4].second); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_transaction.cpp b/fuse_core/test/test_transaction.cpp index d42a94395..17c6b3ccd 100644 --- a/fuse_core/test/test_transaction.cpp +++ b/fuse_core/test/test_transaction.cpp @@ -34,8 +34,8 @@ #include #include #include -#include -#include +#include "example_constraint.h" +#include "example_variable.h" #include @@ -912,9 +912,3 @@ TEST(Transaction, Serialize) EXPECT_TRUE(testAddedVariables(expected.addedVariables(), actual)); EXPECT_TRUE(testRemovedVariables(expected.removedVariables(), actual)); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 30a8c5234..ad96329de 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include #include @@ -71,9 +70,3 @@ TEST(Util, wrapAngle2D) EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle - 3.0 * 2.0 * M_PI)); } } - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_uuid.cpp b/fuse_core/test/test_uuid.cpp index 4b39a734e..61500d121 100644 --- a/fuse_core/test/test_uuid.cpp +++ b/fuse_core/test/test_uuid.cpp @@ -216,9 +216,3 @@ TEST(UUID, CollisionManyThreads) } } } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index cf09540b1..4c3f3b48c 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "example_variable.h" #include @@ -41,9 +41,3 @@ TEST(Variable, Type) ExampleVariable variable; ASSERT_EQ("ExampleVariable", variable.type()); } - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/fuse_core/test/throttled_callback.test b/fuse_core/test/throttled_callback.test deleted file mode 100644 index b0596970b..000000000 --- a/fuse_core/test/throttled_callback.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_graphs/benchmark/benchmark_create_problem.cpp b/fuse_graphs/benchmark/benchmark_create_problem.cpp index 491f17c5e..85b4647fe 100644 --- a/fuse_graphs/benchmark/benchmark_create_problem.cpp +++ b/fuse_graphs/benchmark/benchmark_create_problem.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include "example_variable.h" #include diff --git a/fuse_graphs/test/test_hash_graph.cpp b/fuse_graphs/test/test_hash_graph.cpp index 8bfa0fff3..b1f1eae1b 100644 --- a/fuse_graphs/test/test_hash_graph.cpp +++ b/fuse_graphs/test/test_hash_graph.cpp @@ -36,10 +36,10 @@ #include #include #include -#include -#include -#include -#include +#include "covariance_constraint.h" +#include "example_constraint.h" +#include "example_loss.h" +#include "example_variable.h" #include diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 6044ea733..146ba4624 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -64,7 +64,10 @@ void Acceleration2D::onInit() params_.loadFromROS(private_node_handle_); throttled_callback_.setThrottlePeriod(params_.throttle_period); - throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(node_->get_clock()); + } if (params_.indices.empty()) { diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index bd10112b7..d08bb27d4 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -70,7 +70,10 @@ void Imu2D::onInit() params_.loadFromROS(private_node_handle_); throttled_callback_.setThrottlePeriod(params_.throttle_period); - throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(node_->get_clock()); + } if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index c071fe394..49a44ead2 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -69,7 +69,10 @@ void Odometry2D::onInit() params_.loadFromROS(private_node_handle_); throttled_callback_.setThrottlePeriod(params_.throttle_period); - throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(node_->get_clock()); + } if (params_.position_indices.empty() && params_.orientation_indices.empty() && diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 0b038da5d..8b7c4ffc6 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -67,7 +67,10 @@ void Pose2D::onInit() params_.loadFromROS(private_node_handle_); throttled_callback_.setThrottlePeriod(params_.throttle_period); - throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(node_->get_clock()); + } if (params_.position_indices.empty() && params_.orientation_indices.empty()) diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 851935e99..9988968ba 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -64,7 +64,10 @@ void Twist2D::onInit() params_.loadFromROS(private_node_handle_); throttled_callback_.setThrottlePeriod(params_.throttle_period); - throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(node_->get_clock()); + } if (params_.linear_indices.empty() && params_.angular_indices.empty()) diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index b986eb02c..6084902b8 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -41,9 +41,9 @@ #include #include -#include -#include -#include +#include "example_constraint.h" +#include "example_variable.h" +#include "example_variable_stamped.h" #include diff --git a/fuse_optimizers/test/test_optimizer.cpp b/fuse_optimizers/test/test_optimizer.cpp index 804e82bba..73e0f92b7 100644 --- a/fuse_optimizers/test/test_optimizer.cpp +++ b/fuse_optimizers/test/test_optimizer.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include -#include +#include "example_optimizer.h" +#include "common.h" #include diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 6185725f4..7fa48e959 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -74,7 +74,10 @@ void SerializedPublisher::onInit() private_node_handle_.getParam("graph_throttle_use_wall_time", graph_throttle_use_wall_time); graph_publisher_throttled_callback_.setThrottlePeriod(graph_throttle_period); - graph_publisher_throttled_callback_.setUseWallTime(graph_throttle_use_wall_time); + + if (!graph_throttle_use_wall_time) { + graph_publisher_throttled_callback_.setClock(node_->get_clock()); + } // Advertise the topics graph_publisher_ = private_node_handle_.advertise("graph", 1, latch);