diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt
index 75be81486..3efc3c602 100644
--- a/fuse_constraints/CMakeLists.txt
+++ b/fuse_constraints/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(fuse_constraints)
 
+# TODO(CH3): Migrate these to ament
 set(build_depends
   fuse_core
   fuse_graphs
@@ -17,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
 find_package(Ceres REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(glog REQUIRED)
 find_package(SuiteSparse REQUIRED COMPONENTS CCOLAMD)
 
 catkin_package(
diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt
index 484a6ac99..d48ce65ea 100644
--- a/fuse_core/CMakeLists.txt
+++ b/fuse_core/CMakeLists.txt
@@ -1,519 +1,515 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(fuse_core)
 
-set(build_depends
-  fuse_msgs
-  pluginlib
-  roscpp
-)
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CXX_STANDARD_REQUIRED YES)
+endif()
+
+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(fuse_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  ${build_depends}
-)
-find_package(Boost REQUIRED COMPONENTS serialization)
 find_package(Ceres REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(libglog REQUIRED)
-
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    ${PROJECT_NAME}
-  CATKIN_DEPENDS
-    ${build_depends}
-  DEPENDS
-    Boost
-    CERES
-    EIGEN3
-    GLOG
-)
+
+include(boost-extras.cmake)
 
 ###########
 ## Build ##
 ###########
-add_compile_options(-Wall -Werror)
 
 ## fuse_core library
-add_library(${PROJECT_NAME}
-  src/async_motion_model.cpp
-  src/async_publisher.cpp
-  src/async_sensor_model.cpp
-  src/ceres_options.cpp
-  src/constraint.cpp
-  src/graph.cpp
-  src/graph_deserializer.cpp
-  src/loss.cpp
-  src/serialization.cpp
-  src/timestamp_manager.cpp
-  src/transaction.cpp
-  src/transaction_deserializer.cpp
-  src/uuid.cpp
-  src/variable.cpp
-)
-add_dependencies(${PROJECT_NAME}
-  ${catkin_EXPORTED_TARGETS}
+add_library(${PROJECT_NAME} SHARED
+#    src/async_motion_model.cpp
+#    src/async_publisher.cpp
+#    src/async_sensor_model.cpp
+#    src/ceres_options.cpp
+    src/constraint.cpp
+    src/graph.cpp
+    src/graph_deserializer.cpp
+    src/loss.cpp
+    src/serialization.cpp
+    src/timestamp_manager.cpp
+    src/transaction.cpp
+    src/transaction_deserializer.cpp
+    src/uuid.cpp
+    src/variable.cpp
 )
-target_include_directories(${PROJECT_NAME}
-  PUBLIC
-    include
-    ${Boost_INCLUDE_DIRS}
-    ${catkin_INCLUDE_DIRS}
-    ${CERES_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIRS}
-    ${GLOG_INCLUDE_DIRS}
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
-target_link_libraries(${PROJECT_NAME}
-  ${Boost_LIBRARIES}
-  ${catkin_LIBRARIES}
-  ${CERES_LIBRARIES}
-  ${GLOG_LIBRARIES}
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Boost::serialization
+  Ceres::ceres
+  Eigen3::Eigen
+  ${fuse_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  ${rcl_interfaces_TARGETS}
 )
-set_target_properties(${PROJECT_NAME}
-  PROPERTIES
-    CXX_STANDARD 14
-    CXX_STANDARD_REQUIRED YES
+target_link_libraries(${PROJECT_NAME} PRIVATE
+  rclcpp_components::component
 )
 
-# fuse_echo executable
-add_executable(fuse_echo
-  src/fuse_echo.cpp
-)
-add_dependencies(fuse_echo
-  ${catkin_EXPORTED_TARGETS}
-)
-target_include_directories(fuse_echo
-  PUBLIC
-    include
-    ${catkin_INCLUDE_DIRS}
-)
-target_link_libraries(fuse_echo
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
-set_target_properties(fuse_echo
-  PROPERTIES
-    CXX_STANDARD 14
-    CXX_STANDARD_REQUIRED YES
+## fuse_echo executable
+add_library(fuse_echo_component SHARED src/fuse_echo.cpp)
+target_link_libraries(fuse_echo_component PUBLIC ${PROJECT_NAME})
+target_link_libraries(fuse_echo_component PRIVATE rclcpp_components::component)
+
+rclcpp_components_register_node(fuse_echo_component
+  PLUGIN "fuse_core::FuseEcho"
+  EXECUTABLE fuse_echo
+  EXECUTOR SingleThreadedExecutor
 )
 
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # find_package(ament_cmake_gtest REQUIRED)
+  # 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
+#   )
+
+#   # Constraint tests
+#   catkin_add_gtest(test_constraint
+#     test/test_constraint.cpp
+#   )
+#   add_dependencies(test_constraint
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_constraint
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${CMAKE_CURRENT_SOURCE_DIR}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_constraint
+#     ${PROJECT_NAME}
+#   )
+#   set_target_properties(test_constraint
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Eigen tests
+#   catkin_add_gtest(test_eigen
+#     test/test_eigen.cpp
+#   )
+#   add_dependencies(test_eigen
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_eigen
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${CMAKE_CURRENT_SOURCE_DIR}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_eigen
+#     ${PROJECT_NAME}
+#   )
+#   set_target_properties(test_eigen
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Local Parameterization tests
+#   catkin_add_gtest(test_local_parameterization
+#     test/test_local_parameterization.cpp
+#   )
+#   target_include_directories(test_local_parameterization
+#     PRIVATE
+#       include
+#   )
+#   target_link_libraries(test_local_parameterization
+#     ${PROJECT_NAME}
+#   )
+#   set_target_properties(test_local_parameterization
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Loss tests
+#   catkin_add_gtest(test_loss
+#     test/test_loss.cpp
+#   )
+#   add_dependencies(test_loss
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_loss
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${CMAKE_CURRENT_SOURCE_DIR}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_loss
+#     ${PROJECT_NAME}
+#   )
+#   set_target_properties(test_loss
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Message Buffer Tests
+#   catkin_add_gtest(test_message_buffer
+#     test/test_message_buffer.cpp
+#   )
+#   add_dependencies(test_message_buffer
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_message_buffer
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_message_buffer
+#     ${catkin_LIBRARIES}
+#   )
+#   set_target_properties(test_message_buffer
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Timestamp Manager Tests
+#   catkin_add_gtest(test_timestamp_manager
+#     test/test_timestamp_manager.cpp
+#   )
+#   add_dependencies(test_timestamp_manager
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_timestamp_manager
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_timestamp_manager
+#     ${PROJECT_NAME}
+#     ${catkin_LIBRARIES}
+#   )
+#   set_target_properties(test_timestamp_manager
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Transaction tests
+#   catkin_add_gtest(test_transaction
+#     test/test_transaction.cpp
+#   )
+#   add_dependencies(test_transaction
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_transaction
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${CMAKE_CURRENT_SOURCE_DIR}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_transaction
+#     ${PROJECT_NAME}
+#     ${catkin_LIBRARIES}
+#   )
+#   set_target_properties(test_transaction
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Parameter tests
+#   add_rostest_gtest(test_parameter
+#     test/parameter.test
+#     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
+#   )
+
+#   # Util tests
+#   catkin_add_gtest(test_util
+#     test/test_util.cpp
+#   )
+#   add_dependencies(test_util
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_util
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_util
+#     ${catkin_LIBRARIES}
+#   )
+#   set_target_properties(test_util
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # UUID tests
+#   catkin_add_gtest(test_uuid
+#     test/test_uuid.cpp
+#   )
+#   add_dependencies(test_uuid
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_uuid
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_uuid
+#     ${PROJECT_NAME}
+#     ${catkin_LIBRARIES}
+#   )
+#   set_target_properties(test_uuid
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+
+#   # Variable tests
+#   catkin_add_gtest(test_variable
+#     test/test_variable.cpp
+#   )
+#   add_dependencies(test_variable
+#     ${catkin_EXPORTED_TARGETS}
+#   )
+#   target_include_directories(test_variable
+#     PRIVATE
+#       include
+#       ${Boost_INCLUDE_DIRS}
+#       ${catkin_INCLUDE_DIRS}
+#       ${CERES_INCLUDE_DIRS}
+#       ${CMAKE_CURRENT_SOURCE_DIR}
+#       ${EIGEN3_INCLUDE_DIRS}
+#   )
+#   target_link_libraries(test_variable
+#     ${PROJECT_NAME}
+#   )
+#   set_target_properties(test_variable
+#     PROPERTIES
+#       CXX_STANDARD 14
+#       CXX_STANDARD_REQUIRED YES
+#   )
+# endif()
+
 #############
 ## Install ##
 #############
 
-install(
-  TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-install(
-  TARGETS fuse_echo
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(TARGETS fuse_echo_component
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-install(
-  DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
-#############
-## Testing ##
-#############
+ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  ament_cmake
+  fuse_msgs
+  pluginlib
+  rcl_interfaces
+  rclcpp
+  rclcpp_components
+  Ceres
+  Eigen3
+)
 
-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
-  )
-
-  # Constraint tests
-  catkin_add_gtest(test_constraint
-    test/test_constraint.cpp
-  )
-  add_dependencies(test_constraint
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_constraint
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_constraint
-    ${PROJECT_NAME}
-  )
-  set_target_properties(test_constraint
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Eigen tests
-  catkin_add_gtest(test_eigen
-    test/test_eigen.cpp
-  )
-  add_dependencies(test_eigen
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_eigen
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_eigen
-    ${PROJECT_NAME}
-  )
-  set_target_properties(test_eigen
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Local Parameterization tests
-  catkin_add_gtest(test_local_parameterization
-    test/test_local_parameterization.cpp
-  )
-  target_include_directories(test_local_parameterization
-    PRIVATE
-      include
-  )
-  target_link_libraries(test_local_parameterization
-    ${PROJECT_NAME}
-  )
-  set_target_properties(test_local_parameterization
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Loss tests
-  catkin_add_gtest(test_loss
-    test/test_loss.cpp
-  )
-  add_dependencies(test_loss
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_loss
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_loss
-    ${PROJECT_NAME}
-  )
-  set_target_properties(test_loss
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Message Buffer Tests
-  catkin_add_gtest(test_message_buffer
-    test/test_message_buffer.cpp
-  )
-  add_dependencies(test_message_buffer
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_message_buffer
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_message_buffer
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_message_buffer
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Timestamp Manager Tests
-  catkin_add_gtest(test_timestamp_manager
-    test/test_timestamp_manager.cpp
-  )
-  add_dependencies(test_timestamp_manager
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_timestamp_manager
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_timestamp_manager
-    ${PROJECT_NAME}
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_timestamp_manager
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Transaction tests
-  catkin_add_gtest(test_transaction
-    test/test_transaction.cpp
-  )
-  add_dependencies(test_transaction
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_transaction
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_transaction
-    ${PROJECT_NAME}
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_transaction
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Parameter tests
-  add_rostest_gtest(test_parameter
-    test/parameter.test
-    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
-  )
-
-  # Util tests
-  catkin_add_gtest(test_util
-    test/test_util.cpp
-  )
-  add_dependencies(test_util
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_util
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_util
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_util
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # UUID tests
-  catkin_add_gtest(test_uuid
-    test/test_uuid.cpp
-  )
-  add_dependencies(test_uuid
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_uuid
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_uuid
-    ${PROJECT_NAME}
-    ${catkin_LIBRARIES}
-  )
-  set_target_properties(test_uuid
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-
-  # Variable tests
-  catkin_add_gtest(test_variable
-    test/test_variable.cpp
-  )
-  add_dependencies(test_variable
-    ${catkin_EXPORTED_TARGETS}
-  )
-  target_include_directories(test_variable
-    PRIVATE
-      include
-      ${Boost_INCLUDE_DIRS}
-      ${catkin_INCLUDE_DIRS}
-      ${CERES_INCLUDE_DIRS}
-      ${CMAKE_CURRENT_SOURCE_DIR}
-      ${EIGEN3_INCLUDE_DIRS}
-  )
-  target_link_libraries(test_variable
-    ${PROJECT_NAME}
-  )
-  set_target_properties(test_variable
-    PROPERTIES
-      CXX_STANDARD 14
-      CXX_STANDARD_REQUIRED YES
-  )
-endif()
+ament_package(CONFIG_EXTRAS boost-extras.cmake)
diff --git a/fuse_core/COLCON_IGNORE b/fuse_core/COLCON_IGNORE
deleted file mode 100644
index e69de29bb..000000000
diff --git a/fuse_core/boost-extras.cmake b/fuse_core/boost-extras.cmake
new file mode 100644
index 000000000..12c9f2f6c
--- /dev/null
+++ b/fuse_core/boost-extras.cmake
@@ -0,0 +1,29 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# 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 Open Source Robotics Foundation, Inc. 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.
+
+find_package(Boost REQUIRED COMPONENTS serialization)
diff --git a/fuse_core/include/fuse_core/console.h b/fuse_core/include/fuse_core/console.h
index 6e905a52e..7f3cb2f45 100644
--- a/fuse_core/include/fuse_core/console.h
+++ b/fuse_core/include/fuse_core/console.h
@@ -34,18 +34,17 @@
 #ifndef FUSE_CORE_CONSOLE_H
 #define FUSE_CORE_CONSOLE_H
 
-#include <ros/console.h>
-#include <ros/time.h>
+#include <rclcpp/clock.hpp>
 
 
 namespace fuse_core
 {
 
 /**
- * @brief ROS console filter that prints messages with ROS_*_DELAYED_THROTTLE and allows to reset the last time the
+ * @brief a log filter that provides a condition to RCLCPP_*_STREAM_EXPRESSION and allows to reset the last time the
  * message was print, so the delayed and throttle conditions are computed from the initial state again.
  */
-class DelayedThrottleFilter : public ros::console::FilterBase
+class DelayedThrottleFilter
 {
 public:
   /**
@@ -68,14 +67,16 @@ class DelayedThrottleFilter : public ros::console::FilterBase
    */
   bool isEnabled() override
   {
-    const auto now = ros::Time::now().toSec();
+    #warn "migrated from ros1, using default clock"
+    const auto now = rclcpp::Clock().now().seconds();
 
     if (last_hit_ < 0.0)
     {
       last_hit_ = now;
+      return true;
     }
 
-    if (ROSCONSOLE_THROTTLE_CHECK(now, last_hit_, period_))
+    if (now > (last_hit_ + period_))
     {
       last_hit_ = now;
       return true;
diff --git a/fuse_core/include/fuse_core/constraint.h b/fuse_core/include/fuse_core/constraint.h
index f876ff84c..8931d78f3 100644
--- a/fuse_core/include/fuse_core/constraint.h
+++ b/fuse_core/include/fuse_core/constraint.h
@@ -48,6 +48,7 @@
 #include <initializer_list>
 #include <memory>
 #include <ostream>
+#include <iostream>
 #include <string>
 #include <utility>
 #include <vector>
diff --git a/fuse_core/include/fuse_core/graph.h b/fuse_core/include/fuse_core/graph.h
index 67e9fa0ab..b6f19ba8f 100644
--- a/fuse_core/include/fuse_core/graph.h
+++ b/fuse_core/include/fuse_core/graph.h
@@ -152,7 +152,7 @@ class Graph
   FUSE_SMART_PTR_ALIASES_ONLY(Graph)
 
   /**
-   * @brief A range of fuse_ros::Constraint objects
+   * @brief A range of fuse_core::Constraint objects
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
@@ -161,7 +161,7 @@ class Graph
   using const_constraint_range = boost::any_range<const Constraint, boost::forward_traversal_tag>;
 
   /**
-   * @brief A range of fuse_ros::Variable objects
+   * @brief A range of fuse_core::Variable objects
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
@@ -379,7 +379,7 @@ class Graph
    * @return            A Ceres Solver Summary structure containing information about the optimization process
    */
   virtual ceres::Solver::Summary optimizeFor(
-    const ros::Duration& max_optimization_time,
+    const rclcpp::Duration& max_optimization_time,
     const ceres::Solver::Options& options = ceres::Solver::Options()) = 0;
 
   /**
diff --git a/fuse_core/include/fuse_core/graph_deserializer.h b/fuse_core/include/fuse_core/graph_deserializer.h
index d4ef1969f..780939d54 100644
--- a/fuse_core/include/fuse_core/graph_deserializer.h
+++ b/fuse_core/include/fuse_core/graph_deserializer.h
@@ -34,11 +34,11 @@
 #ifndef FUSE_CORE_GRAPH_DESERIALIZER_H
 #define FUSE_CORE_GRAPH_DESERIALIZER_H
 
-#include <fuse_msgs/SerializedGraph.h>
+#include <fuse_msgs/msg/serialized_graph.hpp>
 #include <fuse_core/constraint.h>
 #include <fuse_core/graph.h>
 #include <fuse_core/variable.h>
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 
 
 namespace fuse_core
@@ -47,7 +47,7 @@ namespace fuse_core
 /**
  * @brief Serialize a graph into a message
  */
-void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::SerializedGraph& msg);
+void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg);
 
 /**
  * @brief Deserialize a graph
@@ -73,7 +73,10 @@ class GraphDeserializer
    * @param[in]  msg  The SerializedGraph message to be deserialized
    * @return          A unique_ptr to a derived Graph object
    */
-  fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::SerializedGraph::ConstPtr& msg) const;
+   inline fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const
+   {
+     return deserialize(*msg);
+   }
 
   /**
    * @brief Deserialize a SerializedGraph message into a fuse Graph object.
@@ -84,7 +87,7 @@ class GraphDeserializer
    * @param[in]  msg  The SerializedGraph message to be deserialized
    * @return          A unique_ptr to a derived Graph object
    */
-  fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::SerializedGraph& msg) const;
+  fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph& msg) const;
 
 private:
   pluginlib::ClassLoader<fuse_core::Variable> variable_loader_;      //!< Pluginlib class loader for Variable types
diff --git a/fuse_core/include/fuse_core/loss.h b/fuse_core/include/fuse_core/loss.h
index 5e1c7e769..1b3ea2ffd 100644
--- a/fuse_core/include/fuse_core/loss.h
+++ b/fuse_core/include/fuse_core/loss.h
@@ -42,9 +42,9 @@
 #include <ceres/loss_function.h>
 
 #include <ostream>
+#include <iostream>
 #include <string>
 
-
 /**
  * @brief Implementation of the clone() member function for derived classes
  *
diff --git a/fuse_core/include/fuse_core/loss_loader.h b/fuse_core/include/fuse_core/loss_loader.h
index 69899d6db..b1d0dc326 100644
--- a/fuse_core/include/fuse_core/loss_loader.h
+++ b/fuse_core/include/fuse_core/loss_loader.h
@@ -35,7 +35,7 @@
 #define FUSE_CORE_LOSS_LOADER_H
 
 #include <fuse_core/loss.h>
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 
 #include <string>
 
diff --git a/fuse_core/include/fuse_core/message_buffer.h b/fuse_core/include/fuse_core/message_buffer.h
index 1c4986d8f..8d41cc96b 100644
--- a/fuse_core/include/fuse_core/message_buffer.h
+++ b/fuse_core/include/fuse_core/message_buffer.h
@@ -35,8 +35,8 @@
 #define FUSE_CORE_MESSAGE_BUFFER_H
 
 #include <fuse_core/fuse_macros.h>
-#include <ros/duration.h>
-#include <ros/time.h>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
 
 #include <boost/range/any_range.hpp>
 
@@ -68,18 +68,18 @@ class MessageBuffer
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
-   * member. When dereferenced, an iterator returns a std::pair<ros::Time, MESSAGE>&.
+   * member. When dereferenced, an iterator returns a std::pair<rclcpp::Time, MESSAGE>&.
    */
-  using message_range = boost::any_range<const std::pair<ros::Time, Message>, boost::forward_traversal_tag>;
+  using message_range = boost::any_range<const std::pair<rclcpp::Time, Message>, boost::forward_traversal_tag>;
 
   /**
    * @brief A range of timestamps
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
-   * member. When dereferenced, an iterator returns a const ros::Time&.
+   * member. When dereferenced, an iterator returns a const rclcpp::Time&.
    */
-  using stamp_range = boost::any_range<const ros::Time, boost::forward_traversal_tag>;
+  using stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>;
 
   /**
    * Constructor
@@ -87,7 +87,7 @@ class MessageBuffer
    * @param[in] buffer_length The length of the message buffer history. If queries arrive involving timestamps
    *                          that are older than the buffer length, an exception will be thrown.
    */
-  explicit MessageBuffer(const ros::Duration& buffer_length = ros::DURATION_MAX);
+  explicit MessageBuffer(const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Destructor
@@ -97,7 +97,7 @@ class MessageBuffer
   /**
    * @brief Read-only access to the buffer length
    */
-  const ros::Duration& bufferLength() const
+  const rclcpp::Duration& bufferLength() const
   {
     return buffer_length_;
   }
@@ -105,7 +105,7 @@ class MessageBuffer
   /**
    * @brief Write access to the buffer length
    */
-  void bufferLength(const ros::Duration& buffer_length)
+  void bufferLength(const rclcpp::Duration& buffer_length)
   {
     buffer_length_ = buffer_length;
   }
@@ -118,7 +118,7 @@ class MessageBuffer
    * @param[in] stamp The stamp to assign to the message
    * @param[in] msg   A message
    */
-  void insert(const ros::Time& stamp, const Message& msg);
+  void insert(const rclcpp::Time& stamp, const Message& msg);
 
   /**
    * @brief Query the buffer for the set of messages between two timestamps
@@ -136,7 +136,7 @@ class MessageBuffer
    *                            \p ending_stamp.
    * @return                    An iterator range containing all of the messages between the specified stamps.
    */
-  message_range query(const ros::Time& beginning_stamp, const ros::Time& ending_stamp, bool extended_range = true);
+  message_range query(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, bool extended_range = true);
 
   /**
    * @brief Read-only access to the current set of timestamps
@@ -146,16 +146,16 @@ class MessageBuffer
   stamp_range stamps() const;
 
 protected:
-  using Buffer = std::deque<std::pair<ros::Time, Message>>;
+  using Buffer = std::deque<std::pair<rclcpp::Time, Message>>;
   Buffer buffer_;  //!< The container of received messages, sorted by timestamp
-  ros::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
+  rclcpp::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
                                  //!< will be removed from the motion model history
 
   /**
    * @brief Helper function used with boost::transform_iterators to convert the internal Buffer value type
-   * into a const ros::Time& iterator compatible with stamp_range
+   * into a const rclcpp::Time& iterator compatible with stamp_range
    */
-  static const ros::Time& extractStamp(const typename Buffer::value_type& element)
+  static const rclcpp::Time& extractStamp(const typename Buffer::value_type& element)
   {
     return element.first;
   }
diff --git a/fuse_core/include/fuse_core/message_buffer_impl.h b/fuse_core/include/fuse_core/message_buffer_impl.h
index d83cd668d..979b34e91 100644
--- a/fuse_core/include/fuse_core/message_buffer_impl.h
+++ b/fuse_core/include/fuse_core/message_buffer_impl.h
@@ -34,8 +34,8 @@
 #ifndef FUSE_CORE_MESSAGE_BUFFER_IMPL_H
 #define FUSE_CORE_MESSAGE_BUFFER_IMPL_H
 
-#include <ros/duration.h>
-#include <ros/time.h>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
 
 #include <boost/iterator/transform_iterator.hpp>
 
@@ -49,13 +49,13 @@ namespace fuse_core
 {
 
 template<class Message>
-MessageBuffer<Message>::MessageBuffer(const ros::Duration& buffer_length) :
+MessageBuffer<Message>::MessageBuffer(const rclcpp::Duration& buffer_length) :
   buffer_length_(buffer_length)
 {
 }
 
 template<class Message>
-void MessageBuffer<Message>::insert(const ros::Time& stamp, const Message& msg)
+void MessageBuffer<Message>::insert(const rclcpp::Time& stamp, const Message& msg)
 {
   buffer_.emplace_back(stamp, msg);
   purgeHistory();
@@ -63,8 +63,8 @@ void MessageBuffer<Message>::insert(const ros::Time& stamp, const Message& msg)
 
 template<class Message>
 typename MessageBuffer<Message>::message_range MessageBuffer<Message>::query(
-  const ros::Time& beginning_stamp,
-  const ros::Time& ending_stamp,
+  const rclcpp::Time& beginning_stamp,
+  const rclcpp::Time& ending_stamp,
   bool extended_range)
 {
   // Verify the query is valid
@@ -131,9 +131,9 @@ template<class Message>
 void MessageBuffer<Message>::purgeHistory()
 {
   // Purge any messages that are more than buffer_length_ seconds older than the most recent entry
-  // A setting of ros::DURATION_MAX means "keep everything"
+  // A setting of rclcpp::Duration::max() means "keep everything"
   // And we want to keep at least two entries in buffer at all times, regardless of the stamps.
-  if ((buffer_length_ == ros::DURATION_MAX) || (buffer_.size() <= 2))
+  if ((buffer_length_ == rclcpp::Duration::max()) || (buffer_.size() <= 2))
   {
     return;
   }
@@ -141,7 +141,7 @@ void MessageBuffer<Message>::purgeHistory()
   // Compute the expiration time carefully, as ROS can't handle negative times
   const auto& ending_stamp = buffer_.back().first;
   auto expiration_time =
-      ending_stamp.toSec() > buffer_length_.toSec() ? ending_stamp - buffer_length_ : ros::Time(0, 0);
+      ending_stamp.toSec() > buffer_length_.toSec() ? ending_stamp - buffer_length_ : rclcpp::Time(0, 0);
   // Remove buffer elements before the expiration time.
   // Be careful to ensure that:
   //  - at least two entries remains at all times
diff --git a/fuse_core/include/fuse_core/serialization.h b/fuse_core/include/fuse_core/serialization.h
index 22deef28f..032029c37 100644
--- a/fuse_core/include/fuse_core/serialization.h
+++ b/fuse_core/include/fuse_core/serialization.h
@@ -36,7 +36,7 @@
 
 #include <fuse_core/uuid.h>
 
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
@@ -147,13 +147,14 @@ namespace serialization
 {
 
 /**
- * @brief Serialize a ros::Time variable using Boost Serialization
+ * @brief Serialize a rclcpp::Time variable using Boost Serialization
  */
 template<class Archive>
-void serialize(Archive& archive, ros::Time& stamp, const unsigned int /* version */)
+void serialize(Archive& archive, rclcpp::Time& stamp, const unsigned int /* version */)
 {
-  archive & stamp.sec;
-  archive & stamp.nsec;
+  #warning "discarding clock source in serialisation"
+  rcl_time_point_value_t time_point = stamp.nanoseconds();
+  archive & time_point;
 }
 
 /**
diff --git a/fuse_core/include/fuse_core/throttled_callback.h b/fuse_core/include/fuse_core/throttled_callback.h
index 4cccad7f3..0a241306f 100644
--- a/fuse_core/include/fuse_core/throttled_callback.h
+++ b/fuse_core/include/fuse_core/throttled_callback.h
@@ -34,7 +34,7 @@
 #ifndef FUSE_CORE_THROTTLED_CALLBACK_H
 #define FUSE_CORE_THROTTLED_CALLBACK_H
 
-#include <ros/subscriber.h>
+//#include <ros/subscriber.h>
 
 #include <functional>
 #include <utility>
@@ -54,17 +54,23 @@ template <class Callback>
 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 ros::WallTime or not. Defaults to false
+   * @param[in] use_wall_time   Whether to use wall time or not. Defaults to false
    */
   ThrottledCallback(Callback&& keep_callback = nullptr,  // NOLINT(whitespace/operators)
                     Callback&& drop_callback = nullptr,  // NOLINT(whitespace/operators)
-                    const ros::Duration& throttle_period = ros::Duration(0.0), const bool use_wall_time = false)
+                    const rclcpp::Duration& throttle_period = rclcpp::Duration(0,0),
+                    const bool use_wall_time = false)
     : keep_callback_(keep_callback)
     , drop_callback_(drop_callback)
     , throttle_period_(throttle_period)
@@ -77,7 +83,7 @@ class ThrottledCallback
    *
    * @return The current throttle period duration in seconds being used
    */
-  const ros::Duration& getThrottlePeriod() const
+  const rclcpp::Duration& getThrottlePeriod() const
   {
     return throttle_period_;
   }
@@ -85,7 +91,7 @@ class ThrottledCallback
   /**
    * @brief Use wall time flag getter
    *
-   * @return True if using ros::WallTime, false otherwise
+   * @return True if using wall time, false otherwise
    */
   bool getUseWallTime() const
   {
@@ -97,7 +103,7 @@ class ThrottledCallback
    *
    * @param[in] throttle_period The new throttle period duration in seconds to use
    */
-  void setThrottlePeriod(const ros::Duration& throttle_period)
+  void setThrottlePeriod(const rclcpp::Duration& throttle_period)
   {
     throttle_period_ = throttle_period;
   }
@@ -105,7 +111,7 @@ class ThrottledCallback
   /**
    * @brief Use wall time flag setter
    *
-   * @param[in] use_wall_time Whether to use ros::WallTime or not
+   * @param[in] use_wall_time Whether to use rclcpp::WallTime or not
    */
   void setUseWallTime(const bool use_wall_time)
   {
@@ -137,7 +143,7 @@ class ThrottledCallback
    *
    * @return The last time the keep callback was called
    */
-  const ros::Time& getLastCalledTime() const
+  const rclcpp::Time& getLastCalledTime() const
   {
     return last_called_time_;
   }
@@ -156,15 +162,16 @@ 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
-    const ros::Time now = use_wall_time_ ? ros::Time(ros::WallTime::now().toSec()) : ros::Time::now();
-    if (last_called_time_.isZero() || throttle_period_.isZero() || now - last_called_time_ > 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_)
     {
       if (keep_callback_)
       {
         keep_callback_(std::forward<Args>(args)...);
       }
 
-      if (last_called_time_.isZero())
+      if (last_called_time_.nanoseconds() == 0)
       {
         last_called_time_ = now;
       }
@@ -193,10 +200,10 @@ class ThrottledCallback
 private:
   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
-  ros::Duration throttle_period_;  //!< The throttling period duration in seconds
-  bool use_wall_time_;             //<! The flag to indicate whether to use ros::WallTime or not
+  rclcpp::Duration throttle_period_;  //!< The throttling period duration in seconds
+  bool use_wall_time_;             //<! The flag to indicate whether to use wall time or not
 
-  ros::Time last_called_time_;  //!< The last time the keep callback was called
+  rclcpp::Time last_called_time_;  //!< The last time the keep callback was called
 };
 
 /**
diff --git a/fuse_core/include/fuse_core/timestamp_manager.h b/fuse_core/include/fuse_core/timestamp_manager.h
index da8895f11..60d94fe65 100644
--- a/fuse_core/include/fuse_core/timestamp_manager.h
+++ b/fuse_core/include/fuse_core/timestamp_manager.h
@@ -38,8 +38,8 @@
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/variable.h>
-#include <ros/duration.h>
-#include <ros/time.h>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
 
 #include <boost/range/any_range.hpp>
 
@@ -80,8 +80,8 @@ class TimestampManager
    * @param[out] variables       One or more variables at both the \p beginning_stamp and \p ending_stamp. The
    *                             variables should include initial values for the optimizer.
    */
-  using MotionModelFunction = std::function<void(const ros::Time& beginning_stamp,
-                                                 const ros::Time& ending_stamp,
+  using MotionModelFunction = std::function<void(const rclcpp::Time& beginning_stamp,
+                                                 const rclcpp::Time& ending_stamp,
                                                  std::vector<Constraint::SharedPtr>& constraints,
                                                  std::vector<Variable::SharedPtr>& variables)>;
 
@@ -90,9 +90,9 @@ class TimestampManager
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
-   * member. When dereferenced, an iterator returns a const ros::Time&.
+   * member. When dereferenced, an iterator returns a const rclcpp::Time&.
    */
-  using const_stamp_range = boost::any_range<const ros::Time, boost::forward_traversal_tag>;
+  using const_stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>;
 
   /**
    * @brief Constructor that accepts the motion model generator as a std::function object, probably constructed using
@@ -102,7 +102,7 @@ class TimestampManager
    * @param[in] buffer_length The length of the motion model history. If queries arrive involving timestamps
    *                          that are older than the buffer length, an exception will be thrown.
    */
-  explicit TimestampManager(MotionModelFunction generator, const ros::Duration& buffer_length = ros::DURATION_MAX);
+  explicit TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Constructor that accepts the motion model generator as a member function pointer and object pointer
@@ -116,12 +116,12 @@ class TimestampManager
    *                          that are older than the buffer length, an exception will be thrown.
    */
   template<class T>
-  TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                const ros::Time& ending_stamp,
+  TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                const rclcpp::Time& ending_stamp,
                                 std::vector<Constraint::SharedPtr>& constraints,
                                 std::vector<Variable::SharedPtr>& variables),
                    T* obj,
-                   const ros::Duration& buffer_length = ros::DURATION_MAX);
+                   const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Constructor that accepts the motion model generator as a const member function pointer and object pointer
@@ -135,12 +135,12 @@ class TimestampManager
    *                          that are older than the buffer length, an exception will be thrown.
    */
   template<class T>
-  TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                const ros::Time& ending_stamp,
+  TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                const rclcpp::Time& ending_stamp,
                                 std::vector<Constraint::SharedPtr>& constraints,
                                 std::vector<Variable::SharedPtr>& variables) const,
                    T* obj,
-                   const ros::Duration& buffer_length = ros::DURATION_MAX);
+                   const rclcpp::Duration& buffer_length = rclcpp::Duration::max());
 
   /**
    * @brief Destructor
@@ -150,7 +150,7 @@ class TimestampManager
   /**
    * @brief Read-only access to the buffer length
    */
-  const ros::Duration& bufferLength() const
+  const rclcpp::Duration& bufferLength() const
   {
     return buffer_length_;
   }
@@ -158,7 +158,7 @@ class TimestampManager
   /**
    * @brief Write access to the buffer length
    */
-  void bufferLength(const ros::Duration& buffer_length)
+  void bufferLength(const rclcpp::Duration& buffer_length)
   {
     buffer_length_ = buffer_length;
   }
@@ -200,16 +200,16 @@ class TimestampManager
    */
   struct MotionModelSegment
   {
-    ros::Time beginning_stamp;
-    ros::Time ending_stamp;
+    rclcpp::Time beginning_stamp;
+    rclcpp::Time ending_stamp;
     std::vector<Constraint::SharedPtr> constraints;
     std::vector<Variable::SharedPtr> variables;
 
     MotionModelSegment() = default;
 
     MotionModelSegment(
-      const ros::Time& beginning_stamp,
-      const ros::Time& ending_stamp,
+      const rclcpp::Time& beginning_stamp,
+      const rclcpp::Time& ending_stamp,
       const std::vector<Constraint::SharedPtr>& constraints,
       const std::vector<Variable::SharedPtr>& variables) :
         beginning_stamp(beginning_stamp),
@@ -226,10 +226,10 @@ class TimestampManager
    * The MotionModelHistory will always contain all represented timestamps; the very last entry will be the ending
    * time of the previous MotionModelSegment, and the very last entry will be an empty MotionModelSegment.
    */
-  using MotionModelHistory = std::map<ros::Time, MotionModelSegment>;
+  using MotionModelHistory = std::map<rclcpp::Time, MotionModelSegment>;
 
   MotionModelFunction generator_;  //!< Users upplied function that generates motion model constraints
-  ros::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
+  rclcpp::Duration buffer_length_;  //!< The length of the motion model history. Segments older than \p buffer_length_
                                  //!< will be removed from the motion model history
   MotionModelHistory motion_model_history_;  //!< Container that stores all previously generated motion models
 
@@ -243,8 +243,8 @@ class TimestampManager
    * @param[out] transaction     A transaction object to be updated with the changes caused by addSegment
    */
   void addSegment(
-    const ros::Time& beginning_stamp,
-    const ros::Time& ending_stamp,
+    const rclcpp::Time& beginning_stamp,
+    const rclcpp::Time& ending_stamp,
     Transaction& transaction);
 
   /**
@@ -271,7 +271,7 @@ class TimestampManager
    */
   void splitSegment(
       MotionModelHistory::iterator& iter,
-      const ros::Time& stamp,
+      const rclcpp::Time& stamp,
       Transaction& transaction);
 
   /**
@@ -281,12 +281,12 @@ class TimestampManager
 };
 
 template<class T>
-TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                                const ros::Time& ending_stamp,
+TimestampManager::TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                                const rclcpp::Time& ending_stamp,
                                                 std::vector<Constraint::SharedPtr>& constraints,
                                                 std::vector<Variable::SharedPtr>& variables),
                                    T* obj,
-                                   const ros::Duration& buffer_length) :
+                                   const rclcpp::Duration& buffer_length) :
   TimestampManager(std::bind(fp,
                              obj,
                              std::placeholders::_1,
@@ -298,12 +298,12 @@ TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp
 }
 
 template<class T>
-TimestampManager::TimestampManager(void(T::*fp)(const ros::Time& beginning_stamp,
-                                                const ros::Time& ending_stamp,
+TimestampManager::TimestampManager(void(T::*fp)(const rclcpp::Time& beginning_stamp,
+                                                const rclcpp::Time& ending_stamp,
                                                 std::vector<Constraint::SharedPtr>& constraints,
                                                 std::vector<Variable::SharedPtr>& variables) const,
                                    T* obj,
-                                   const ros::Duration& buffer_length) :
+                                   const rclcpp::Duration& buffer_length) :
   TimestampManager(std::bind(fp,
                              obj,
                              std::placeholders::_1,
diff --git a/fuse_core/include/fuse_core/transaction.h b/fuse_core/include/fuse_core/transaction.h
index 89ad5fbd8..14bd43213 100644
--- a/fuse_core/include/fuse_core/transaction.h
+++ b/fuse_core/include/fuse_core/transaction.h
@@ -39,7 +39,7 @@
 #include <fuse_core/serialization.h>
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/range/any_range.hpp>
 #include <boost/serialization/access.hpp>
@@ -48,6 +48,7 @@
 #include <boost/serialization/vector.hpp>
 
 #include <ostream>
+#include <iostream>
 #include <set>
 #include <vector>
 
@@ -78,13 +79,13 @@ class Transaction
   using const_constraint_range = boost::any_range<const Constraint, boost::forward_traversal_tag>;
 
   /**
-   * @brief A range of ros::Time objects
+   * @brief A range of rclcpp::Time objects
    *
    * An object representing a range defined by two iterators. It has begin() and end() methods (which means it can
    * be used in range-based for loops), an empty() method, and a front() method for directly accessing the first
-   * member. When dereferenced, an iterator returns a const ros::Time&.
+   * member. When dereferenced, an iterator returns a const rclcpp::Time&.
    */
-  using const_stamp_range = boost::any_range<const ros::Time, boost::forward_traversal_tag>;
+  using const_stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>;
 
   /**
    * @brief A range of UUID objects
@@ -107,12 +108,12 @@ class Transaction
   /**
    * @brief Read-only access to this transaction's timestamp
    */
-  const ros::Time& stamp() const { return stamp_; }
+  const rclcpp::Time& stamp() const { return stamp_; }
 
   /**
    * @brief Write access to this transaction's timestamp
    */
-  void stamp(const ros::Time& stamp) { stamp_ = stamp; }
+  void stamp(const rclcpp::Time& stamp) { stamp_ = stamp; }
 
   /**
    * @brief Read-only access to the set of timestamps involved in this transaction
@@ -127,7 +128,7 @@ class Transaction
    *
    * @return The minimum (oldest) timestamp.
    */
-  const ros::Time& minStamp() const;
+  const rclcpp::Time& minStamp() const;
 
   /**
    * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all involved
@@ -135,7 +136,7 @@ class Transaction
    *
    * @return The maximum (newest) timestamp.
    */
-  const ros::Time& maxStamp() const;
+  const rclcpp::Time& maxStamp() const;
 
   /**
    * @brief Read-only access to the added constraints
@@ -180,7 +181,7 @@ class Transaction
    *
    * @param[in] stamp The timestamp to be added
    */
-  void addInvolvedStamp(const ros::Time& stamp);
+  void addInvolvedStamp(const rclcpp::Time& stamp);
 
   /**
    * @brief Add a constraint to this transaction
@@ -282,10 +283,10 @@ class Transaction
   void deserialize(fuse_core::TextInputArchive& /* archive */);
 
 private:
-  ros::Time stamp_;  //!< The transaction message timestamp
+  rclcpp::Time stamp_;  //!< The transaction message timestamp
   std::vector<Constraint::SharedPtr> added_constraints_;  //!< The constraints to be added
   std::vector<Variable::SharedPtr> added_variables_;  //!< The variables to be added
-  std::set<ros::Time> involved_stamps_;  //!< The set of timestamps involved in this transaction
+  std::set<rclcpp::Time> involved_stamps_;  //!< The set of timestamps involved in this transaction
   std::vector<UUID> removed_constraints_;  //!< The constraint UUIDs to be removed
   std::vector<UUID> removed_variables_;  //!< The variable UUIDs to be removed
 
diff --git a/fuse_core/include/fuse_core/transaction_deserializer.h b/fuse_core/include/fuse_core/transaction_deserializer.h
index c478f0c3b..5ddfaf9b3 100644
--- a/fuse_core/include/fuse_core/transaction_deserializer.h
+++ b/fuse_core/include/fuse_core/transaction_deserializer.h
@@ -34,11 +34,11 @@
 #ifndef FUSE_CORE_TRANSACTION_DESERIALIZER_H
 #define FUSE_CORE_TRANSACTION_DESERIALIZER_H
 
-#include <fuse_msgs/SerializedTransaction.h>
+#include <fuse_msgs/msg/serialized_transaction.hpp>
 #include <fuse_core/constraint.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/variable.h>
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 
 
 namespace fuse_core
@@ -47,7 +47,7 @@ namespace fuse_core
 /**
  * @brief Serialize a transaction into a message
  */
-void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::SerializedTransaction& msg);
+void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg);
 
 /**
  * @brief Deserialize a Transaction
@@ -73,7 +73,7 @@ class TransactionDeserializer
    * @param[IN]  msg  The SerializedTransaction message to be deserialized
    * @return          A fuse Transaction object
    */
-  fuse_core::Transaction deserialize(const fuse_msgs::SerializedTransaction::ConstPtr& msg) const;
+  fuse_core::Transaction deserialize(const fuse_msgs::msg::SerializedTransaction::SharedPtr msg) const;
 
   /**
    * @brief Deserialize a SerializedTransaction message into a fuse Transaction object.
@@ -84,7 +84,7 @@ class TransactionDeserializer
    * @param[IN]  msg  The SerializedTransaction message to be deserialized
    * @return          A fuse Transaction object
    */
-  fuse_core::Transaction deserialize(const fuse_msgs::SerializedTransaction& msg) const;
+  fuse_core::Transaction deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const;
 
 private:
   pluginlib::ClassLoader<fuse_core::Variable> variable_loader_;      //!< Pluginlib class loader for Variable types
diff --git a/fuse_core/include/fuse_core/util.h b/fuse_core/include/fuse_core/util.h
index 2e8a7f98e..2f6d7ce25 100644
--- a/fuse_core/include/fuse_core/util.h
+++ b/fuse_core/include/fuse_core/util.h
@@ -34,8 +34,8 @@
 #ifndef FUSE_CORE_UTIL_H
 #define FUSE_CORE_UTIL_H
 
-#include <ros/console.h>
-#include <ros/node_handle.h>
+//#include <ros/console.h>
+//#include <ros/node_handle.h>
 
 #include <ceres/jet.h>
 #include <Eigen/Core>
diff --git a/fuse_core/include/fuse_core/uuid.h b/fuse_core/include/fuse_core/uuid.h
index ea4fed3e1..53e8c1fdd 100644
--- a/fuse_core/include/fuse_core/uuid.h
+++ b/fuse_core/include/fuse_core/uuid.h
@@ -34,7 +34,7 @@
 #ifndef FUSE_CORE_UUID_H
 #define FUSE_CORE_UUID_H
 
-#include <ros/time.h>
+#include <rclcpp/time.hpp>
 
 #include <boost/functional/hash.hpp>
 #include <boost/uuid/uuid.hpp>
@@ -152,10 +152,10 @@ namespace uuid
    * Every unique timestamp will generate a unique UUID
    *
    * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs
-   * @param[in] stamp            A ROS::Time timestamp
+   * @param[in] stamp            A rclcpp::Time timestamp
    * @return                     A repeatable UUID specific to the provided namespace and timestamp
    */
-  UUID generate(const std::string& namespace_string, const ros::Time& stamp);
+  UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp);
 
   /**
    * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id
@@ -163,11 +163,11 @@ namespace uuid
    * Every unique timestamp and id pair will generate a unique UUID
    *
    * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs
-   * @param[in] stamp            A ROS::Time timestamp
+   * @param[in] stamp            A rclcpp::Time timestamp
    * @param[in] id               A UUID
    * @return                     A repeatable UUID specific to the provided namespace and timestamp
    */
-  UUID generate(const std::string& namespace_string, const ros::Time& stamp, const UUID& id);
+  UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id);
 
     /**
    * @brief Generate a UUID from a namespace string and a user provided id
diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h
index 593a2c26f..2bac497fb 100644
--- a/fuse_core/include/fuse_core/variable.h
+++ b/fuse_core/include/fuse_core/variable.h
@@ -44,6 +44,7 @@
 
 #include <limits>
 #include <ostream>
+#include <iostream>
 #include <string>
 
 
diff --git a/fuse_core/package.xml b/fuse_core/package.xml
index a42e0a591..a7fc29842 100644
--- a/fuse_core/package.xml
+++ b/fuse_core/package.xml
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>fuse_core</name>
   <version>0.4.2</version>
   <description>
@@ -11,17 +12,23 @@
   <author email="swilliams@locusrobotics.com">Stephen Williams</author>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
   <depend>libceres-dev</depend>
+  <depend>libgoogle-glog-dev</depend>
   <depend>eigen</depend>
   <depend>fuse_msgs</depend>
   <depend>pluginlib</depend>
-  <depend>roscpp</depend>
-  <depend version_gte="1.13.8">rosconsole</depend>
-  <test_depend>roslint</test_depend>
-  <test_depend>rostest</test_depend>
+  <depend>rcl_interfaces</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
+    <build_type>ament_cmake</build_type>
     <rosdoc config="rosdoc.yaml" />
   </export>
 </package>
diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp
index 8fa8661f8..5e1794130 100644
--- a/fuse_core/src/fuse_echo.cpp
+++ b/fuse_core/src/fuse_echo.cpp
@@ -31,64 +31,67 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#include <fuse_msgs/SerializedGraph.h>
-#include <fuse_msgs/SerializedTransaction.h>
+#include <fuse_msgs/msg/serialized_graph.h>
+#include <fuse_msgs/msg/serialized_transaction.h>
 #include <fuse_core/graph.h>
 #include <fuse_core/graph_deserializer.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/transaction_deserializer.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/clock.hpp>
 
 
+namespace fuse_core
+{
+
 /**
  * Class that subscribes to the 'graph' and 'transaction' topics and prints the objects to stdout
  */
-class FuseEcho
+class FuseEcho : public rclcpp::Node
 {
 public:
-  explicit FuseEcho(const ros::NodeHandle& node_handle = ros::NodeHandle()) :
-    node_handle_(node_handle)
+  explicit FuseEcho(rclcpp::NodeOptions options): Node("fuse_echo", options)
   {
     // Subscribe to the constraint topic
-    graph_subscriber_ = node_handle_.subscribe("graph", 100, &FuseEcho::graphCallback, this);
-    transaction_subscriber_ = node_handle_.subscribe("transaction", 100, &FuseEcho::transactionCallback, this);
+    graph_subscriber_ = this->create_subscription<fuse_msgs::msg::SerializedGraph>(
+      "graph",
+      rclcpp::QoS(100),
+      std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1)
+    );
+    transaction_subscriber_ = this->create_subscription<fuse_msgs::msg::SerializedTransaction>(
+      "transaction",
+      rclcpp::QoS(100),
+      std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1)
+    );
   }
 
 private:
   fuse_core::GraphDeserializer graph_deserializer_;
   fuse_core::TransactionDeserializer transaction_deserializer_;
-  ros::NodeHandle node_handle_;
-  ros::Subscriber graph_subscriber_;
-  ros::Subscriber transaction_subscriber_;
+  rclcpp::Subscription<fuse_msgs::msg::SerializedGraph>::SharedPtr graph_subscriber_;
+  rclcpp::Subscription<fuse_msgs::msg::SerializedTransaction>::SharedPtr transaction_subscriber_;
 
-  void graphCallback(const fuse_msgs::SerializedGraph::ConstPtr& msg)
+  void graphCallback(const fuse_msgs::msg::SerializedGraph& msg)
   {
     std::cout << "-------------------------" << std::endl;
     std::cout << "GRAPH:" << std::endl;
-    std::cout << "received at: " << ros::Time::now() << std::endl;
+    std::cout << "received at: " << this->now().seconds() << std::endl;
     auto graph = graph_deserializer_.deserialize(msg);
     graph->print();
   }
 
-  void transactionCallback(const fuse_msgs::SerializedTransaction::ConstPtr& msg)
+  void transactionCallback(const fuse_msgs::msg::SerializedTransaction& msg)
   {
     std::cout << "-------------------------" << std::endl;
     std::cout << "TRANSACTION:" << std::endl;
-    std::cout << "received at: " << ros::Time::now() << std::endl;
+    std::cout << "received at: " << this->now().seconds() << std::endl;
     auto transaction = transaction_deserializer_.deserialize(msg);
     transaction.print();
   }
 };
 
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "fuse_echo", ros::init_options::AnonymousName);
-
-  // Object that subscribes to the 'graph' and 'transaction' topics and prints the objects to stdout
-  FuseEcho echoer;
+} // namespace fuse_core
 
-  // Wait for an exit signal
-  ros::spin();
 
-  return 0;
-}
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(fuse_core::FuseEcho)
diff --git a/fuse_core/src/graph_deserializer.cpp b/fuse_core/src/graph_deserializer.cpp
index 2f9fcfb89..75e9e5ffc 100644
--- a/fuse_core/src/graph_deserializer.cpp
+++ b/fuse_core/src/graph_deserializer.cpp
@@ -34,7 +34,6 @@
 #include <fuse_core/graph_deserializer.h>
 
 #include <fuse_core/serialization.h>
-#include <fuse_msgs/SerializedGraph.h>
 
 #include <boost/iostreams/stream.hpp>
 
@@ -42,7 +41,7 @@
 namespace fuse_core
 {
 
-void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::SerializedGraph& msg)
+void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg)
 {
   // Serialize the graph into the msg.data field
   boost::iostreams::stream<fuse_core::MessageBufferStreamSink> stream(msg.data);
@@ -78,12 +77,7 @@ GraphDeserializer::GraphDeserializer() :
   }
 }
 
-fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::SerializedGraph::ConstPtr& msg) const
-{
-  return deserialize(*msg);
-}
-
-fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::SerializedGraph& msg) const
+fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::msg::SerializedGraph& msg) const
 {
   // Create a Graph object using pluginlib. This will throw if the plugin name is not found.
   // The unique ptr returned by pluginlib has a custom deleter. This makes it annoying to return
diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp
index d8d00aacf..37480dced 100644
--- a/fuse_core/src/timestamp_manager.cpp
+++ b/fuse_core/src/timestamp_manager.cpp
@@ -33,12 +33,6 @@
  */
 #include <fuse_core/timestamp_manager.h>
 
-#include <fuse_core/constraint.h>
-#include <fuse_core/transaction.h>
-#include <fuse_core/variable.h>
-#include <ros/duration.h>
-#include <ros/time.h>
-
 #include <boost/iterator/transform_iterator.hpp>
 
 #include <algorithm>
@@ -46,13 +40,12 @@
 #include <set>
 #include <stdexcept>
 #include <utility>
-#include <vector>
 
 
 namespace fuse_core
 {
 
-TimestampManager::TimestampManager(MotionModelFunction generator, const ros::Duration& buffer_length) :
+TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) :
   generator_(generator),
   buffer_length_(buffer_length)
 {
@@ -70,7 +63,7 @@ void TimestampManager::query(
   }
   // Verify the query is within the buffer length
   if ( (!motion_model_history_.empty())
-    && (buffer_length_ != ros::DURATION_MAX)
+    && (buffer_length_ != rclcpp::Duration::max())
     && (stamps.front() < motion_model_history_.begin()->first)
     && (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_)))
   {
@@ -79,7 +72,7 @@ void TimestampManager::query(
   // Create a list of all the required timestamps involved in motion model segments that must be created
   // Add all of the existing timestamps between the first and last input stamp
   Transaction motion_model_transaction;
-  std::set<ros::Time> augmented_stamps(stamps.begin(), stamps.end());
+  std::set<rclcpp::Time> augmented_stamps(stamps.begin(), stamps.end());
   auto first_stamp = *augmented_stamps.begin();
   auto last_stamp = *augmented_stamps.rbegin();
   {
@@ -99,14 +92,14 @@ void TimestampManager::query(
     }
   }
   // Convert the sequence of stamps into stamp pairs that must be generated
-  std::vector<std::pair<ros::Time, ros::Time>> stamp_pairs;
+  std::vector<std::pair<rclcpp::Time, rclcpp::Time>> stamp_pairs;
   {
     for (auto previous_iter = augmented_stamps.begin(), current_iter = std::next(augmented_stamps.begin());
          current_iter != augmented_stamps.end();
          ++previous_iter, ++current_iter)
     {
-      const ros::Time& previous_stamp = *previous_iter;
-      const ros::Time& current_stamp = *current_iter;
+      const rclcpp::Time& previous_stamp = *previous_iter;
+      const rclcpp::Time& current_stamp = *current_iter;
       // Check if the timestamp pair is exactly an existing pair. If so, don't add it.
       auto history_iter = motion_model_history_.lower_bound(previous_stamp);
       if ((history_iter != motion_model_history_.end()) &&
@@ -173,7 +166,7 @@ void TimestampManager::query(
 
 TimestampManager::const_stamp_range TimestampManager::stamps() const
 {
-  auto extract_stamp = +[](const MotionModelHistory::value_type& element) -> const ros::Time&
+  auto extract_stamp = +[](const MotionModelHistory::value_type& element) -> const rclcpp::Time&
   {
     return element.first;
   };
@@ -183,8 +176,8 @@ TimestampManager::const_stamp_range TimestampManager::stamps() const
 }
 
 void TimestampManager::addSegment(
-  const ros::Time& beginning_stamp,
-  const ros::Time& ending_stamp,
+  const rclcpp::Time& beginning_stamp,
+  const rclcpp::Time& ending_stamp,
   Transaction& transaction)
 {
   // Generate the set of constraints and variables to add
@@ -228,11 +221,11 @@ void TimestampManager::removeSegment(
 
 void TimestampManager::splitSegment(
     MotionModelHistory::iterator& iter,
-    const ros::Time& stamp,
+    const rclcpp::Time& stamp,
     Transaction& transaction)
 {
-  ros::Time removed_beginning_stamp = iter->second.beginning_stamp;
-  ros::Time removed_ending_stamp = iter->second.ending_stamp;
+  rclcpp::Time removed_beginning_stamp = iter->second.beginning_stamp;
+  rclcpp::Time removed_ending_stamp = iter->second.ending_stamp;
   // We need to remove the existing constraint.
   removeSegment(iter, transaction);
   // And add a new constraint from the beginning of the removed constraint to the provided stamp
@@ -244,9 +237,9 @@ void TimestampManager::splitSegment(
 void TimestampManager::purgeHistory()
 {
   // Purge any motion model segments that are more than buffer_length_ seconds older than the most recent entry
-  // A setting of ros::DURATION_MAX means "keep everything"
+  // A setting of rclcpp::Duration::max() means "keep everything"
   // And we want to keep at least one entry in motion model history, regardless of the stamps.
-  if ((buffer_length_ == ros::DURATION_MAX) || (motion_model_history_.size() <= 1))
+  if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1))
   {
     return;
   }
@@ -254,7 +247,7 @@ void TimestampManager::purgeHistory()
   // (a) are left with only one entry, OR
   // (b) the time delta between the beginning and end is within the buffer_length_
   // We compare with the ending timestamp of each segment to be conservative
-  ros::Time ending_stamp = motion_model_history_.rbegin()->first;
+  rclcpp::Time ending_stamp = motion_model_history_.rbegin()->first;
   while ( (motion_model_history_.size() > 1)
       && ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_))
   {
diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp
index 353022766..339e1f157 100644
--- a/fuse_core/src/transaction.cpp
+++ b/fuse_core/src/transaction.cpp
@@ -33,23 +33,18 @@
  */
 #include <fuse_core/transaction.h>
 
-#include <fuse_core/constraint.h>
-#include <fuse_core/uuid.h>
-#include <fuse_core/variable.h>
-#include <ros/time.h>
 
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/range/empty.hpp>
 
 #include <algorithm>
-#include <ostream>
 #include <utility>
 
 
 namespace fuse_core
 {
 
-const ros::Time& Transaction::minStamp() const
+const rclcpp::Time& Transaction::minStamp() const
 {
   if (involved_stamps_.empty())
   {
@@ -61,7 +56,7 @@ const ros::Time& Transaction::minStamp() const
   }
 }
 
-const ros::Time& Transaction::maxStamp() const
+const rclcpp::Time& Transaction::maxStamp() const
 {
   if (involved_stamps_.empty())
   {
@@ -73,7 +68,7 @@ const ros::Time& Transaction::maxStamp() const
   }
 }
 
-void Transaction::addInvolvedStamp(const ros::Time& stamp)
+void Transaction::addInvolvedStamp(const rclcpp::Time& stamp)
 {
   involved_stamps_.insert(stamp);
 }
@@ -236,11 +231,11 @@ void Transaction::merge(const Transaction& other, bool overwrite)
 
 void Transaction::print(std::ostream& stream) const
 {
-  stream << "Stamp: " << stamp_ << "\n";
+  stream << "Stamp: " << stamp_.nanoseconds() << "\n";
   stream << "Involved Timestamps:\n";
   for (const auto& involved_stamp : involved_stamps_)
   {
-    stream << " - " << involved_stamp << "\n";
+    stream << " - " << involved_stamp.nanoseconds() << "\n";
   }
   stream << "Added Variables:\n";
   for (const auto& added_variable : added_variables_)
diff --git a/fuse_core/src/transaction_deserializer.cpp b/fuse_core/src/transaction_deserializer.cpp
index aa4bb9e3c..8a026b684 100644
--- a/fuse_core/src/transaction_deserializer.cpp
+++ b/fuse_core/src/transaction_deserializer.cpp
@@ -34,7 +34,6 @@
 #include <fuse_core/transaction_deserializer.h>
 
 #include <fuse_core/serialization.h>
-#include <fuse_msgs/SerializedTransaction.h>
 
 #include <boost/iostreams/stream.hpp>
 
@@ -42,7 +41,7 @@
 namespace fuse_core
 {
 
-void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::SerializedTransaction& msg)
+void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg)
 {
   // Serialize the transaction into the msg.data field
   boost::iostreams::stream<fuse_core::MessageBufferStreamSink> stream(msg.data);
@@ -75,12 +74,12 @@ TransactionDeserializer::TransactionDeserializer() :
   }
 }
 
-fuse_core::Transaction TransactionDeserializer::deserialize(const fuse_msgs::SerializedTransaction::ConstPtr& msg) const
+fuse_core::Transaction TransactionDeserializer::deserialize(const fuse_msgs::msg::SerializedTransaction::SharedPtr msg) const
 {
   return deserialize(*msg);
 }
 
-fuse_core::Transaction TransactionDeserializer::deserialize(const fuse_msgs::SerializedTransaction& msg) const
+fuse_core::Transaction TransactionDeserializer::deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const
 {
   // The Transaction object is not a plugin and has no derived types. That makes it much easier to use.
   auto transaction = fuse_core::Transaction();
diff --git a/fuse_core/src/uuid.cpp b/fuse_core/src/uuid.cpp
index a25a71b7f..129ca7cb1 100644
--- a/fuse_core/src/uuid.cpp
+++ b/fuse_core/src/uuid.cpp
@@ -33,16 +33,10 @@
  */
 #include <fuse_core/uuid.h>
 
-#include <ros/time.h>
-
-#include <boost/uuid/uuid.hpp>
-#include <boost/uuid/uuid_io.hpp>
-
 #include <algorithm>
 #include <array>
 #include <mutex>
 #include <random>
-#include <string>
 
 
 namespace fuse_core
@@ -65,30 +59,47 @@ UUID generate()
   return uuid;
 }
 
-UUID generate(const std::string& namespace_string, const ros::Time& stamp)
+UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp)
 {
-  constexpr size_t buffer_size = sizeof(stamp.sec) + sizeof(stamp.nsec);
+  // replicate ros1 behaviour for now
+  #warning "deprecated time API"
+  rcl_time_point_value_t stamp_val = stamp.nanoseconds();
+  int32_t stamp_s = RCUTILS_NS_TO_S(stamp_val);
+  int32_t stamp_ns = stamp_val % (1000LL * 1000LL * 1000LL);
+
+  constexpr size_t buffer_size = sizeof(stamp_s) + sizeof(stamp_ns);
   std::array<unsigned char, buffer_size> buffer;
   auto iter = buffer.begin();
-  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp.sec),
-                   reinterpret_cast<const unsigned char*>(&stamp.sec) + sizeof(stamp.sec),
+
+  #warning "unsafe time packing"
+  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp_s),
+                   reinterpret_cast<const unsigned char*>(&stamp_s) + sizeof(stamp_s),
                    iter);
-  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp.nsec),
-                   reinterpret_cast<const unsigned char*>(&stamp.nsec) + sizeof(stamp.nsec),
+  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp_ns),
+                   reinterpret_cast<const unsigned char*>(&stamp_ns) + sizeof(stamp_ns),
                    iter);
   return generate(namespace_string, buffer.data(), buffer.size());
 }
 
-UUID generate(const std::string& namespace_string, const ros::Time& stamp, const UUID& id)
+UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id)
 {
-  constexpr size_t buffer_size = sizeof(stamp.sec) + sizeof(stamp.nsec) + UUID::static_size();
+  // replicate ros1 behaviour for now
+  #warning "deprecated time API"
+  rcl_time_point_value_t stamp_val = stamp.nanoseconds();
+  int32_t stamp_s = RCUTILS_NS_TO_S(stamp_val);
+  int32_t stamp_ns = stamp_val % (1000LL * 1000LL * 1000LL);
+
+  constexpr size_t buffer_size = sizeof(stamp_s) + sizeof(stamp_ns) + UUID::static_size();
   std::array<unsigned char, buffer_size> buffer;
   auto iter = buffer.begin();
-  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp.sec),
-                   reinterpret_cast<const unsigned char*>(&stamp.sec) + sizeof(stamp.sec),
+
+
+  #warning "unsafe time packing"
+  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp_s),
+                   reinterpret_cast<const unsigned char*>(&stamp_s) + sizeof(stamp_s),
                    iter);
-  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp.nsec),
-                   reinterpret_cast<const unsigned char*>(&stamp.nsec) + sizeof(stamp.nsec),
+  iter = std::copy(reinterpret_cast<const unsigned char*>(&stamp_ns),
+                   reinterpret_cast<const unsigned char*>(&stamp_ns) + sizeof(stamp_ns),
                    iter);
   iter = std::copy(id.begin(),
                    id.end(),
diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp
index 28d744f09..96bc4e69f 100644
--- a/fuse_models/src/odometry_2d_publisher.cpp
+++ b/fuse_models/src/odometry_2d_publisher.cpp
@@ -336,7 +336,8 @@ void Odometry2DPublisher::publishTimerCallback(const ros::TimerEvent& event)
 
   if (latest_stamp == Synchronizer::TIME_ZERO)
   {
-    ROS_WARN_STREAM_FILTER(&delayed_throttle_filter_, "No valid state data yet. Delaying tf broadcast.");
+    RCLCPP_WARN_STREAM_EXPRESSION(node_->get_logger(), delayed_throttle_filter_.isEnabled(),
+                                  "No valid state data yet. Delaying tf broadcast.");
     return;
   }
 
diff --git a/fuse_msgs/CMakeLists.txt b/fuse_msgs/CMakeLists.txt
index 1f2715e06..d189b9a63 100644
--- a/fuse_msgs/CMakeLists.txt
+++ b/fuse_msgs/CMakeLists.txt
@@ -17,8 +17,7 @@ set(msg_files
   "msg/SerializedTransaction.msg"
 )
 
-rosidl_generate_interfaces(${PROJECT_NAME}
-    ${msg_files}
+rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files}
   DEPENDENCIES
     std_msgs
   ADD_LINTER_TESTS
diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h
index dadf1847f..1f7938dd5 100644
--- a/fuse_optimizers/include/fuse_optimizers/optimizer.h
+++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h
@@ -41,7 +41,7 @@
 #include <fuse_core/publisher.h>
 #include <fuse_core/sensor_model.h>
 #include <fuse_core/transaction.h>
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 #include <ros/ros.h>
 
 #include <string>