fuse -> ROS 2 fuse_core : Parameters and Tests (#286)

Co-authored-by: Shane Loretz <[email protected]>
Co-authored-by: Ivor Wanders <[email protected]>

.gitignore

@@ -30,3 +30,6 @@
 *.exe
 *.out
 *.app
+
+# Python compiled bytecode files
+*.pyc

fuse_constraints/test/test_normal_delta_pose_2d.cpp

@@ -34,7 +34,7 @@
 #include <fuse_constraints/normal_delta_pose_2d.h>
 #include <fuse_constraints/normal_delta_pose_2d_cost_functor.h>
 
-#include <test/cost_function_gtest.h>
+#include "cost_function_gtest.h"
 
 #include <gtest/gtest.h>
 #include <fuse_core/eigen_gtest.h>

fuse_constraints/test/test_normal_prior_pose_2d.cpp

@@ -34,7 +34,7 @@
 #include <fuse_constraints/normal_prior_pose_2d.h>
 #include <fuse_constraints/normal_prior_pose_2d_cost_functor.h>
 
-#include <test/cost_function_gtest.h>
+#include "cost_function_gtest.h"
 
 #include <gtest/gtest.h>
 #include <fuse_core/eigen_gtest.h>

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 ##
 #############

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
    *

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

fuse_core/include/fuse_core/async_sensor_model.h

@@ -40,8 +40,6 @@
 #include <fuse_core/graph.h>
 #include <fuse_core/callback_wrapper.h>
 
-//#include <fuse_core/fuse_macros.h>
-
 #include <functional>
 #include <string>
 
@@ -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

fuse_core/include/fuse_core/autodiff_local_parameterization.h

@@ -190,7 +190,7 @@ bool AutoDiffLocalParameterization<PlusFunctor, MinusFunctor, kGlobalSize, kLoca
   return ceres::internal::AutoDiff<PlusFunctor, double, kGlobalSize, kLocalSize>
     ::Differentiate(*plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
 #else
-  return ceres::internal::AutoDifferentiate<ceres::internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
+  return ceres::internal::AutoDifferentiate<kGlobalSize, ceres::internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
       *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
 #endif
 }
@@ -217,7 +217,8 @@ bool AutoDiffLocalParameterization<PlusFunctor, MinusFunctor, kGlobalSize, kLoca
   return ceres::internal::AutoDiff<MinusFunctor, double, kGlobalSize, kGlobalSize>
     ::Differentiate(*minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs);
 #else
-  return ceres::internal::AutoDifferentiate<ceres::internal::StaticParameterDims<kGlobalSize, kGlobalSize>>(
+  using StaticParameters = ceres::internal::StaticParameterDims<kGlobalSize, kGlobalSize>;
+  return ceres::internal::AutoDifferentiate<kLocalSize, StaticParameters>(
       *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs);
 #endif
 }