diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt
index 75be81486..670b6337e 100644
--- a/fuse_constraints/CMakeLists.txt
+++ b/fuse_constraints/CMakeLists.txt
@@ -78,7 +78,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -135,7 +135,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_absolute_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -161,7 +161,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_absolute_orientation_3d_stamped_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -187,7 +187,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_absolute_orientation_3d_stamped_euler_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -211,7 +211,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_absolute_pose_2d_stamped_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -235,7 +235,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_absolute_pose_3d_stamped_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -257,7 +257,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_marginal_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -280,7 +280,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_marginalize_variables
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -304,7 +304,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_normal_delta_pose_2d
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -328,7 +328,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_normal_prior_pose_2d
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -352,7 +352,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_relative_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -376,7 +376,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_relative_pose_2d_stamped_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -400,7 +400,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_relative_pose_3d_stamped_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -419,7 +419,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_uuid_ordering
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -436,7 +436,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_variable_constraints
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -458,7 +458,7 @@ if(CATKIN_ENABLE_TESTING)
       )
       set_target_properties(benchmark_normal_delta_pose_2d
         PROPERTIES
-          CXX_STANDARD 14
+          CXX_STANDARD 17
           CXX_STANDARD_REQUIRED YES
       )
     endif()
@@ -477,7 +477,7 @@ if(CATKIN_ENABLE_TESTING)
       )
       set_target_properties(benchmark_normal_prior_pose_2d
         PROPERTIES
-          CXX_STANDARD 14
+          CXX_STANDARD 17
           CXX_STANDARD_REQUIRED YES
       )
     endif()
diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp
index fb767654c..e5496d362 100644
--- a/fuse_constraints/src/absolute_constraint.cpp
+++ b/fuse_constraints/src/absolute_constraint.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_constraints/absolute_constraint.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp
index 34765b2d5..97386d106 100644
--- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp
+++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/absolute_orientation_3d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_prior_orientation_3d_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp
index 6758c831c..bf052e80d 100644
--- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp
+++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h>
 
 #include <fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp
index 31c2044bd..1f3168579 100644
--- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp
+++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/absolute_pose_2d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_prior_pose_2d.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp
index ffabb271e..74c1acf88 100644
--- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp
+++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/absolute_pose_3d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_prior_pose_3d_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp
index 3e85498d3..c08e855b4 100644
--- a/fuse_constraints/src/marginal_constraint.cpp
+++ b/fuse_constraints/src/marginal_constraint.cpp
@@ -35,7 +35,7 @@
 
 #include <fuse_constraints/marginal_cost_function.h>
 #include <fuse_core/constraint.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <Eigen/Core>
diff --git a/fuse_constraints/src/relative_constraint.cpp b/fuse_constraints/src/relative_constraint.cpp
index 1b9e63a92..7b3a890b3 100644
--- a/fuse_constraints/src/relative_constraint.cpp
+++ b/fuse_constraints/src/relative_constraint.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_constraints/relative_constraint.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp
index 73f522e3c..2efd3bf87 100644
--- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp
+++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/relative_orientation_3d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_delta_orientation_3d_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp
index af9ddc156..9b91bc535 100644
--- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp
+++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/relative_pose_2d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_delta_pose_2d.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp
index 1e688e1d5..7ff80f319 100644
--- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp
+++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_constraints/relative_pose_3d_stamped_constraint.h>
 
 #include <fuse_constraints/normal_delta_pose_3d_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt
index 484a6ac99..85bee662f 100644
--- a/fuse_core/CMakeLists.txt
+++ b/fuse_core/CMakeLists.txt
@@ -72,7 +72,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -94,7 +94,7 @@ target_link_libraries(fuse_echo
 )
 set_target_properties(fuse_echo
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -154,7 +154,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_async_motion_model
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -180,7 +180,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_async_publisher
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -206,7 +206,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_async_sensor_model
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -231,7 +231,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_callback_wrapper
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -256,7 +256,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_constraint
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -281,7 +281,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_eigen
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -298,7 +298,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_local_parameterization
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -323,7 +323,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_loss
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -347,7 +347,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_message_buffer
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -372,7 +372,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_timestamp_manager
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -398,7 +398,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_transaction
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -423,7 +423,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_parameter
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -439,7 +439,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_throttled_callback
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -463,7 +463,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_util
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -488,7 +488,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_uuid
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -513,7 +513,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_variable
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 endif()
diff --git a/fuse_core/include/fuse_core/graph_deserializer.h b/fuse_core/include/fuse_core/graph_deserializer.h
index d4ef1969f..04d0a6073 100644
--- a/fuse_core/include/fuse_core/graph_deserializer.h
+++ b/fuse_core/include/fuse_core/graph_deserializer.h
@@ -38,7 +38,7 @@
 #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
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/transaction_deserializer.h b/fuse_core/include/fuse_core/transaction_deserializer.h
index c478f0c3b..ec105e029 100644
--- a/fuse_core/include/fuse_core/transaction_deserializer.h
+++ b/fuse_core/include/fuse_core/transaction_deserializer.h
@@ -38,7 +38,7 @@
 #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
diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt
index 7bcbea337..b81f46d5f 100644
--- a/fuse_graphs/CMakeLists.txt
+++ b/fuse_graphs/CMakeLists.txt
@@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -109,7 +109,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_hash_graph
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -135,7 +135,7 @@ if(CATKIN_ENABLE_TESTING)
     )
     set_target_properties(benchmark_create_problem
       PROPERTIES
-        CXX_STANDARD 14
+        CXX_STANDARD 17
         CXX_STANDARD_REQUIRED YES
     )
   endif()
diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp
index 810ca9652..b5d421cbc 100644
--- a/fuse_graphs/src/hash_graph.cpp
+++ b/fuse_graphs/src/hash_graph.cpp
@@ -34,7 +34,7 @@
 #include <fuse_graphs/hash_graph.h>
 
 #include <fuse_core/uuid.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/CMakeLists.txt b/fuse_loss/CMakeLists.txt
index f71d4c341..edbb52ef2 100644
--- a/fuse_loss/CMakeLists.txt
+++ b/fuse_loss/CMakeLists.txt
@@ -58,7 +58,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -123,7 +123,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_loss_function
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -147,7 +147,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_composed_loss
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -171,7 +171,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_huber_loss
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -195,7 +195,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_tukey_loss
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -227,7 +227,7 @@ if(CATKIN_ENABLE_TESTING)
     )
     set_target_properties(test_qwt_loss_plot
       PROPERTIES
-        CXX_STANDARD 14
+        CXX_STANDARD 17
         CXX_STANDARD_REQUIRED YES
     )
 
@@ -257,7 +257,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_scaled_loss
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 endif()
diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp
index 47520d783..9a275439c 100644
--- a/fuse_loss/src/arctan_loss.cpp
+++ b/fuse_loss/src/arctan_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/arctan_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp
index e27261b6a..3b2d027c8 100644
--- a/fuse_loss/src/cauchy_loss.cpp
+++ b/fuse_loss/src/cauchy_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/cauchy_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp
index 28a69a5bb..554503a21 100644
--- a/fuse_loss/src/composed_loss.cpp
+++ b/fuse_loss/src/composed_loss.cpp
@@ -35,7 +35,7 @@
 #include <fuse_loss/trivial_loss.h>
 
 #include <fuse_core/parameter.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp
index 8d24cd672..3bd51c2fa 100644
--- a/fuse_loss/src/dcs_loss.cpp
+++ b/fuse_loss/src/dcs_loss.cpp
@@ -34,7 +34,7 @@
 #include <fuse_loss/dcs_loss.h>
 #include <fuse_loss/loss_function.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp
index bebfe95b7..0ab13738c 100644
--- a/fuse_loss/src/fair_loss.cpp
+++ b/fuse_loss/src/fair_loss.cpp
@@ -34,7 +34,7 @@
 #include <fuse_loss/fair_loss.h>
 #include <fuse_loss/loss_function.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp
index f34d8ef3c..6924ad3ea 100644
--- a/fuse_loss/src/geman_mcclure_loss.cpp
+++ b/fuse_loss/src/geman_mcclure_loss.cpp
@@ -34,7 +34,7 @@
 #include <fuse_loss/geman_mcclure_loss.h>
 #include <fuse_loss/loss_function.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp
index db68d3c7a..b2d3a7877 100644
--- a/fuse_loss/src/huber_loss.cpp
+++ b/fuse_loss/src/huber_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/huber_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp
index f38d5d3ac..f70d33baf 100644
--- a/fuse_loss/src/scaled_loss.cpp
+++ b/fuse_loss/src/scaled_loss.cpp
@@ -34,7 +34,7 @@
 #include <fuse_loss/scaled_loss.h>
 
 #include <fuse_core/parameter.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp
index 8a4fcbf22..fdf7f2909 100644
--- a/fuse_loss/src/softlone_loss.cpp
+++ b/fuse_loss/src/softlone_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/softlone_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp
index d57277e82..426a2fc4a 100644
--- a/fuse_loss/src/tolerant_loss.cpp
+++ b/fuse_loss/src/tolerant_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/tolerant_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/trivial_loss.cpp b/fuse_loss/src/trivial_loss.cpp
index b21defaa6..63877209f 100644
--- a/fuse_loss/src/trivial_loss.cpp
+++ b/fuse_loss/src/trivial_loss.cpp
@@ -33,7 +33,7 @@
  */
 #include <fuse_loss/trivial_loss.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp
index b9f74fc8d..d12753615 100644
--- a/fuse_loss/src/tukey_loss.cpp
+++ b/fuse_loss/src/tukey_loss.cpp
@@ -35,7 +35,7 @@
 
 #include <fuse_core/ceres_macros.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp
index fb5c72105..6780c30b9 100644
--- a/fuse_loss/src/welsch_loss.cpp
+++ b/fuse_loss/src/welsch_loss.cpp
@@ -34,7 +34,7 @@
 #include <fuse_loss/welsch_loss.h>
 #include <fuse_loss/loss_function.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/node_handle.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt
index 597706e0a..9b861f5b9 100644
--- a/fuse_models/CMakeLists.txt
+++ b/fuse_models/CMakeLists.txt
@@ -113,7 +113,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -164,7 +164,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_unicycle_2d
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -180,7 +180,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_unicycle_2d_predict
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -197,7 +197,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_unicycle_2d_state_cost_function
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -223,7 +223,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_graph_ignition
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -239,7 +239,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_unicycle_2d_ignition
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -254,7 +254,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_sensor_proc
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -275,7 +275,7 @@ if(CATKIN_ENABLE_TESTING)
       )
       set_target_properties(benchmark_unicycle_2d_state_cost_function
         PROPERTIES
-          CXX_STANDARD 14
+          CXX_STANDARD 17
           CXX_STANDARD_REQUIRED YES
       )
     endif()
diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp
index 46019e89b..63e314325 100644
--- a/fuse_models/src/acceleration_2d.cpp
+++ b/fuse_models/src/acceleration_2d.cpp
@@ -38,7 +38,7 @@
 #include <fuse_core/uuid.h>
 
 #include <geometry_msgs/AccelWithCovarianceStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 
diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp
index 5977b34ae..80d5b56f1 100644
--- a/fuse_models/src/graph_ignition.cpp
+++ b/fuse_models/src/graph_ignition.cpp
@@ -36,7 +36,7 @@
 
 #include <std_srvs/Empty.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/range/algorithm.hpp>
 #include <boost/range/empty.hpp>
diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp
index 5c3bd12fc..0874faccc 100644
--- a/fuse_models/src/imu_2d.cpp
+++ b/fuse_models/src/imu_2d.cpp
@@ -40,7 +40,7 @@
 #include <geometry_msgs/AccelWithCovarianceStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TwistWithCovarianceStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 #include <sensor_msgs/Imu.h>
 
diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp
index 4298089ac..3ffb04b28 100644
--- a/fuse_models/src/odometry_2d.cpp
+++ b/fuse_models/src/odometry_2d.cpp
@@ -40,7 +40,7 @@
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TwistWithCovarianceStamped.h>
 #include <nav_msgs/Odometry.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 #include <memory>
diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp
index 28d744f09..448443cff 100644
--- a/fuse_models/src/odometry_2d_publisher.cpp
+++ b/fuse_models/src/odometry_2d_publisher.cpp
@@ -41,7 +41,7 @@
 
 #include <geometry_msgs/AccelWithCovarianceStamped.h>
 #include <nav_msgs/Odometry.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <tf2_2d/tf2_2d.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/buffer.h>
diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp
index 026a79741..248e1ded1 100644
--- a/fuse_models/src/pose_2d.cpp
+++ b/fuse_models/src/pose_2d.cpp
@@ -38,7 +38,7 @@
 #include <fuse_core/uuid.h>
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 #include <memory>
diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp
index fa4b80d16..e81d55fb2 100644
--- a/fuse_models/src/transaction.cpp
+++ b/fuse_models/src/transaction.cpp
@@ -34,7 +34,7 @@
 
 #include <fuse_models/transaction.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 // Register this sensor model with ROS as a plugin.
diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp
index 964165a4f..5acf674db 100644
--- a/fuse_models/src/twist_2d.cpp
+++ b/fuse_models/src/twist_2d.cpp
@@ -38,7 +38,7 @@
 #include <fuse_core/uuid.h>
 
 #include <geometry_msgs/TwistWithCovarianceStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 
diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp
index 103459fc9..1da323519 100644
--- a/fuse_models/src/unicycle_2d.cpp
+++ b/fuse_models/src/unicycle_2d.cpp
@@ -48,7 +48,7 @@
 #include <fuse_variables/velocity_angular_2d_stamped.h>
 #include <fuse_variables/velocity_linear_2d_stamped.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 #include <tf2/utils.h>
 
diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp
index 914518196..692be13c4 100644
--- a/fuse_models/src/unicycle_2d_ignition.cpp
+++ b/fuse_models/src/unicycle_2d_ignition.cpp
@@ -50,7 +50,7 @@
 #include <fuse_variables/stamped.h>
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <std_srvs/Empty.h>
 #include <tf2/convert.h>
 #include <tf2/LinearMath/Quaternion.h>
diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp
index 6d4d30e6e..1f9505a46 100644
--- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp
+++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp
@@ -39,7 +39,7 @@
 #include <fuse_variables/position_2d_stamped.h>
 #include <fuse_variables/velocity_angular_2d_stamped.h>
 #include <fuse_variables/velocity_linear_2d_stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <Eigen/Dense>
diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt
index 798787394..3bb31f6f7 100644
--- a/fuse_optimizers/CMakeLists.txt
+++ b/fuse_optimizers/CMakeLists.txt
@@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -72,7 +72,7 @@ target_link_libraries(batch_optimizer_node
 )
 set_target_properties(batch_optimizer_node
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -94,7 +94,7 @@ target_link_libraries(fixed_lag_smoother_node
 )
 set_target_properties(fixed_lag_smoother_node
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -153,7 +153,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_variable_stamp_index
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -177,7 +177,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_optimizer
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -209,7 +209,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_fixed_lag_ignition
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 endif()
diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h
index 20f16f736..0fa6928a2 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>
diff --git a/fuse_publishers/CMakeLists.txt b/fuse_publishers/CMakeLists.txt
index 68885d310..a2f78f010 100644
--- a/fuse_publishers/CMakeLists.txt
+++ b/fuse_publishers/CMakeLists.txt
@@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -114,7 +114,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_path_2d_publisher
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -137,7 +137,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_pose_2d_publisher
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -158,7 +158,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_stamped_variable_synchronizer
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 endif()
diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp
index f1903285f..96ca9a1b1 100644
--- a/fuse_publishers/src/path_2d_publisher.cpp
+++ b/fuse_publishers/src/path_2d_publisher.cpp
@@ -41,7 +41,7 @@
 #include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <nav_msgs/Path.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 #include <tf2/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp
index 6c99b228d..506423a04 100644
--- a/fuse_publishers/src/pose_2d_publisher.cpp
+++ b/fuse_publishers/src/pose_2d_publisher.cpp
@@ -42,7 +42,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 #include <tf2/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp
index 85ecb9f44..db1e20bc8 100644
--- a/fuse_publishers/src/serialized_publisher.cpp
+++ b/fuse_publishers/src/serialized_publisher.cpp
@@ -41,7 +41,7 @@
 #include <fuse_core/transaction_deserializer.h>
 #include <fuse_msgs/SerializedGraph.h>
 #include <fuse_msgs/SerializedTransaction.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 
 
diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt
index 269c8f32f..57da1db9d 100644
--- a/fuse_tutorials/CMakeLists.txt
+++ b/fuse_tutorials/CMakeLists.txt
@@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -72,7 +72,7 @@ target_link_libraries(range_sensor_simulator
 )
 set_target_properties(range_sensor_simulator
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp
index a42e6a3b0..cbd800403 100644
--- a/fuse_tutorials/src/beacon_publisher.cpp
+++ b/fuse_tutorials/src/beacon_publisher.cpp
@@ -37,7 +37,7 @@
 #include <fuse_core/graph.h>
 #include <fuse_core/transaction.h>
 #include <fuse_variables/point_2d_landmark.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp
index 885c8e0cd..42971c993 100644
--- a/fuse_tutorials/src/range_constraint.cpp
+++ b/fuse_tutorials/src/range_constraint.cpp
@@ -34,7 +34,7 @@
 #include <fuse_tutorials/range_constraint.h>
 
 #include <fuse_tutorials/range_cost_functor.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <ceres/autodiff_cost_function.h>
diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp
index 9e9e32030..1b0595412 100644
--- a/fuse_tutorials/src/range_sensor_model.cpp
+++ b/fuse_tutorials/src/range_sensor_model.cpp
@@ -40,7 +40,7 @@
 #include <fuse_tutorials/range_constraint.h>
 #include <fuse_variables/point_2d_landmark.h>
 #include <fuse_variables/position_2d_stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
 
diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt
index fe74f100e..ca25103e7 100644
--- a/fuse_variables/CMakeLists.txt
+++ b/fuse_variables/CMakeLists.txt
@@ -63,7 +63,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
@@ -120,7 +120,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_acceleration_angular_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -144,7 +144,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_acceleration_angular_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -168,7 +168,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_acceleration_linear_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -192,7 +192,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_acceleration_linear_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -215,7 +215,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_fixed_size_variable
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -239,7 +239,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_load_device_id
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -263,7 +263,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_orientation_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -287,7 +287,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_orientation_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -311,7 +311,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_point_2d_fixed_landmark
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -335,7 +335,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_point_2d_landmark
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -359,7 +359,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_point_3d_fixed_landmark
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -383,7 +383,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_point_3d_landmark
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -407,7 +407,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_position_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -431,7 +431,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_position_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -455,7 +455,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_velocity_angular_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -479,7 +479,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_velocity_angular_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -503,7 +503,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_velocity_linear_2d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 
@@ -527,7 +527,7 @@ if(CATKIN_ENABLE_TESTING)
   )
   set_target_properties(test_velocity_linear_3d_stamped
     PROPERTIES
-      CXX_STANDARD 14
+      CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
 endif()
diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp
index 5d4bd545d..c65f1144b 100644
--- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp
+++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp
index 710be3098..37e80468d 100644
--- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp
+++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp
index 089dcbdaa..037e562ef 100644
--- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp
+++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp
index 31297d1c7..e86a0f17a 100644
--- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp
+++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp
index 6abb54314..f5848c48f 100644
--- a/fuse_variables/src/orientation_2d_stamped.cpp
+++ b/fuse_variables/src/orientation_2d_stamped.cpp
@@ -37,7 +37,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp
index 462eea00e..f5dea4a83 100644
--- a/fuse_variables/src/orientation_3d_stamped.cpp
+++ b/fuse_variables/src/orientation_3d_stamped.cpp
@@ -37,7 +37,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp
index ab00f94d3..b8dbc31ff 100644
--- a/fuse_variables/src/point_2d_fixed_landmark.cpp
+++ b/fuse_variables/src/point_2d_fixed_landmark.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
 #include <fuse_variables/fixed_size_variable.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp
index 603b46ed2..b971a5a20 100644
--- a/fuse_variables/src/point_2d_landmark.cpp
+++ b/fuse_variables/src/point_2d_landmark.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
 #include <fuse_variables/fixed_size_variable.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp
index 7b92113f5..254a9f0c2 100644
--- a/fuse_variables/src/point_3d_fixed_landmark.cpp
+++ b/fuse_variables/src/point_3d_fixed_landmark.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
 #include <fuse_variables/fixed_size_variable.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp
index 7743b13d4..184c0d5ed 100644
--- a/fuse_variables/src/point_3d_landmark.cpp
+++ b/fuse_variables/src/point_3d_landmark.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_core/variable.h>
 #include <fuse_variables/fixed_size_variable.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 #include <boost/serialization/export.hpp>
 
diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp
index 0f7411f45..e653937f7 100644
--- a/fuse_variables/src/position_2d_stamped.cpp
+++ b/fuse_variables/src/position_2d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp
index 66efcccbb..65dd23a0e 100644
--- a/fuse_variables/src/position_3d_stamped.cpp
+++ b/fuse_variables/src/position_3d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp
index 5da8cf624..176808256 100644
--- a/fuse_variables/src/velocity_angular_2d_stamped.cpp
+++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp
index 6756716ff..5c565589e 100644
--- a/fuse_variables/src/velocity_angular_3d_stamped.cpp
+++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp
index 8dae97c31..505368111 100644
--- a/fuse_variables/src/velocity_linear_2d_stamped.cpp
+++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp
index 901aaa0a4..8661c7ec6 100644
--- a/fuse_variables/src/velocity_linear_3d_stamped.cpp
+++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp
@@ -36,7 +36,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_variables/fixed_size_variable.h>
 #include <fuse_variables/stamped.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <ros/time.h>
 
 #include <boost/serialization/export.hpp>
diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt
index aa9a0ec44..bde535755 100644
--- a/fuse_viz/CMakeLists.txt
+++ b/fuse_viz/CMakeLists.txt
@@ -73,6 +73,9 @@ add_dependencies(${PROJECT_NAME}
 target_include_directories(${PROJECT_NAME}
   PUBLIC
     include
+)
+target_include_directories(${PROJECT_NAME}
+  SYSTEM PUBLIC
     ${Boost_INCLUDE_DIRS}
     ${catkin_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIRS}
@@ -85,7 +88,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 set_target_properties(${PROJECT_NAME}
   PROPERTIES
-    CXX_STANDARD 14
+    CXX_STANDARD 17
     CXX_STANDARD_REQUIRED YES
 )
 
diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp
index f95bbc8c6..90679f5e2 100644
--- a/fuse_viz/src/serialized_graph_display.cpp
+++ b/fuse_viz/src/serialized_graph_display.cpp
@@ -322,5 +322,5 @@ void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::Co
 
 }  // namespace rviz
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS(rviz::SerializedGraphDisplay, rviz::Display)