diff --git a/.clang-tidy b/.clang-tidy
index 181fbf69c..ed398ab43 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -17,7 +17,6 @@ Checks:          'performance-*,
                   readability-identifier-naming,
                   '
 HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
-AnalyzeTemporaryDtors: false
 CheckOptions:
   - key:             llvm-namespace-comment.ShortNamespaceLines
     value:           '10'
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
index 565ab2474..c4a75ec2f 100644
--- a/capabilities/src/execute_task_solution_capability.cpp
+++ b/capabilities/src/execute_task_solution_capability.cpp
@@ -36,13 +36,13 @@
 
 #include "execute_task_solution_capability.h"
 
-#include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/plan_execution/plan_execution.h>
-#include <moveit/trajectory_processing/trajectory_tools.h>
-#include <moveit/kinematic_constraints/utils.h>
-#include <moveit/move_group/capability_names.h>
-#include <moveit/robot_state/conversions.h>
-#include <moveit/utils/message_checks.h>
+#include <moveit/moveit_cpp/moveit_cpp.hpp>
+#include <moveit/plan_execution/plan_execution.hpp>
+#include <moveit/trajectory_processing/trajectory_tools.hpp>
+#include <moveit/kinematic_constraints/utils.hpp>
+#include <moveit/move_group/capability_names.hpp>
+#include <moveit/robot_state/conversions.hpp>
+#include <moveit/utils/message_checks.hpp>
 #include <fmt/format.h>
 
 namespace {
diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h
index 26b437c53..0dfd12d13 100644
--- a/capabilities/src/execute_task_solution_capability.h
+++ b/capabilities/src/execute_task_solution_capability.h
@@ -41,7 +41,7 @@
 
 #pragma once
 
-#include <moveit/move_group/move_group_capability.h>
+#include <moveit/move_group/move_group_capability.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 
 #include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h
index c9fb61f2c..6c16aef41 100644
--- a/core/include/moveit/task_constructor/container_p.h
+++ b/core/include/moveit/task_constructor/container_p.h
@@ -39,7 +39,7 @@
 #pragma once
 
 #include <moveit/task_constructor/container.h>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include "stage_p.h"
 
 #include <boost/bimap.hpp>
diff --git a/core/include/moveit/task_constructor/introspection.h b/core/include/moveit/task_constructor/introspection.h
index 10652d329..cf4dfb764 100644
--- a/core/include/moveit/task_constructor/introspection.h
+++ b/core/include/moveit/task_constructor/introspection.h
@@ -38,7 +38,7 @@
 
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit_task_constructor_msgs/msg/task_description.hpp>
 #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h
index 2ac47927f..cded66e16 100644
--- a/core/include/moveit/task_constructor/marker_tools.h
+++ b/core/include/moveit/task_constructor/marker_tools.h
@@ -2,7 +2,7 @@
 
 #include <rviz_marker_tools/marker_creation.h>
 #include <visualization_msgs/msg/marker.hpp>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <functional>
 
 namespace planning_scene {
diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h
index 734418f1a..6ddeab7c9 100644
--- a/core/include/moveit/task_constructor/merge.h
+++ b/core/include/moveit/task_constructor/merge.h
@@ -37,9 +37,9 @@
 #pragma once
 
 #include <moveit/task_constructor/storage.h>
-#include <moveit/robot_model/robot_model.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
-#include <moveit/trajectory_processing/time_parameterization.h>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/trajectory_processing/time_parameterization.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h
index f48935070..4427249e1 100644
--- a/core/include/moveit/task_constructor/moveit_compat.h
+++ b/core/include/moveit/task_constructor/moveit_compat.h
@@ -38,8 +38,8 @@
 
 #pragma once
 
-#include <moveit/version.h>
-#include <moveit/macros/class_forward.h>
+#include <moveit/version.hpp>
+#include <moveit/macros/class_forward.hpp>
 
 #define MOVEIT_VERSION_GE(major, minor, patch)                                                \
 	(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h
index 0d4ef8a46..c84b6a270 100644
--- a/core/include/moveit/task_constructor/solvers/cartesian_path.h
+++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h
@@ -39,7 +39,7 @@
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
-#include <moveit/robot_state/cartesian_interpolator.h>
+#include <moveit/robot_state/cartesian_interpolator.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h
index 0adc6979c..8adc28984 100644
--- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h
+++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h
@@ -39,7 +39,7 @@
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h
index a2bf1428a..88a15510d 100644
--- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h
+++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h
@@ -43,7 +43,7 @@
 #include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
 #include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
 #include <rclcpp/node.hpp>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 
 namespace planning_pipeline {
 MOVEIT_CLASS_FORWARD(PlanningPipeline);
diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h
index 33f9e627e..361db751a 100644
--- a/core/include/moveit/task_constructor/solvers/planner_interface.h
+++ b/core/include/moveit/task_constructor/solvers/planner_interface.h
@@ -38,7 +38,7 @@
 
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit_msgs/msg/constraints.hpp>
 #include <moveit/task_constructor/properties.h>
 #include <Eigen/Geometry>
diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h
index 74fb3941e..c7573c13c 100644
--- a/core/include/moveit/task_constructor/stage.h
+++ b/core/include/moveit/task_constructor/stage.h
@@ -41,7 +41,7 @@
 
 #include "trajectory_execution_info.h"
 #include "utils.h"
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit/task_constructor/storage.h>
 #include <vector>
 #include <list>
diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h
index 823d85322..1d10cef94 100644
--- a/core/include/moveit/task_constructor/stages/fix_collision_objects.h
+++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h
@@ -41,7 +41,7 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/stage.h>
 #include <geometry_msgs/msg/vector3.hpp>
-#include <moveit/collision_detection/collision_common.h>
+#include <moveit/collision_detection/collision_common.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/include/moveit/task_constructor/stages/noop.h b/core/include/moveit/task_constructor/stages/noop.h
index 02f41994f..9f770c773 100644
--- a/core/include/moveit/task_constructor/stages/noop.h
+++ b/core/include/moveit/task_constructor/stages/noop.h
@@ -36,7 +36,7 @@
 #pragma once
 
 #include <moveit/task_constructor/stage.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h
index 8ee67e4ee..621010fb1 100644
--- a/core/include/moveit/task_constructor/stages/pick.h
+++ b/core/include/moveit/task_constructor/stages/pick.h
@@ -36,7 +36,7 @@
 
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit/task_constructor/container.h>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 
diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h
index 7fe83cd22..29960cbd6 100644
--- a/core/include/moveit/task_constructor/stages/simple_grasp.h
+++ b/core/include/moveit/task_constructor/stages/simple_grasp.h
@@ -37,7 +37,7 @@
 #pragma once
 
 #include <moveit/task_constructor/container.h>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <Eigen/Geometry>
 
diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h
index 0d8e350a0..0dac14662 100644
--- a/core/include/moveit/task_constructor/storage.h
+++ b/core/include/moveit/task_constructor/storage.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/cost_queue.h>
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h
index fae0f7212..8351bc4e7 100644
--- a/core/include/moveit/task_constructor/task.h
+++ b/core/include/moveit/task_constructor/task.h
@@ -44,10 +44,10 @@
 #include <moveit/task_constructor/introspection.h>
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 
 #include <moveit_msgs/msg/move_it_error_codes.hpp>
-#include <moveit/utils/moveit_error_code.h>
+#include <moveit/utils/moveit_error_code.hpp>
 
 #include <rclcpp/node.hpp>
 
diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h
index a2b30ec6c..d1463fabc 100644
--- a/core/include/moveit/task_constructor/utils.h
+++ b/core/include/moveit/task_constructor/utils.h
@@ -44,7 +44,7 @@
 
 #include <Eigen/Geometry>
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 
 namespace planning_scene {
 MOVEIT_CLASS_FORWARD(PlanningScene);
diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp
index 7be6c0187..5825d0679 100644
--- a/core/python/bindings/src/core.cpp
+++ b/core/python/bindings/src/core.cpp
@@ -40,9 +40,9 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/task.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
+#include <moveit/move_group_interface/move_group_interface.hpp>
 
 namespace py = pybind11;
 using namespace py::literals;
diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h
index 9be7a4de6..9fa71a6e9 100644
--- a/core/python/bindings/src/core.h
+++ b/core/python/bindings/src/core.h
@@ -38,7 +38,7 @@
 #include <moveit/task_constructor/container.h>
 #include <moveit/task_constructor/cost_queue.h>
 #include <moveit/task_constructor/cost_terms.h>
-#include <moveit/utils/moveit_error_code.h>
+#include <moveit/utils/moveit_error_code.hpp>
 #include <pybind11/smart_holder.h>
 
 /** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp
index 508225130..6ebb1e80e 100644
--- a/core/python/bindings/src/stages.cpp
+++ b/core/python/bindings/src/stages.cpp
@@ -37,7 +37,7 @@
 #include <moveit/task_constructor/stages.h>
 #include <moveit/task_constructor/stages/pick.h>
 #include <moveit/task_constructor/stages/simple_grasp.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <moveit_msgs/msg/planning_scene.hpp>
 #include <pybind11/stl.h>
 #include <py_binding_tools/ros_msg_typecasters.h>
diff --git a/core/src/container.cpp b/core/src/container.cpp
index 826ed005b..7269a8236 100644
--- a/core/src/container.cpp
+++ b/core/src/container.cpp
@@ -38,8 +38,8 @@
 #include <moveit/task_constructor/introspection.h>
 #include <moveit/task_constructor/merge.h>
 #include <moveit/task_constructor/fmt_p.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
 
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp
index 4afeffe97..179731495 100644
--- a/core/src/cost_terms.cpp
+++ b/core/src/cost_terms.cpp
@@ -38,10 +38,10 @@
 #include <moveit/task_constructor/stage.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/collision_detection/collision_common.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/conversions.h>
+#include <moveit/collision_detection/collision_common.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/conversions.hpp>
 
 #include <Eigen/Geometry>
 
diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp
index 15a2509a7..ebd3bfee7 100644
--- a/core/src/introspection.cpp
+++ b/core/src/introspection.cpp
@@ -44,7 +44,7 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <sstream>
 #include <boost/bimap.hpp>
diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp
index 0ec7ea4ab..5903a6c08 100644
--- a/core/src/marker_tools.cpp
+++ b/core/src/marker_tools.cpp
@@ -1,6 +1,6 @@
 #include <moveit/task_constructor/marker_tools.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/robot_state.hpp>
 
 namespace vm = visualization_msgs;
 
diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp
index 91c1f8983..a6663e9dc 100644
--- a/core/src/solvers/cartesian_path.cpp
+++ b/core/src/solvers/cartesian_path.cpp
@@ -38,10 +38,10 @@
 
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/task_constructor/utils.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/trajectory_processing/time_parameterization.h>
-#include <moveit/kinematics_base/kinematics_base.h>
-#include <moveit/robot_state/cartesian_interpolator.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/trajectory_processing/time_parameterization.hpp>
+#include <moveit/kinematics_base/kinematics_base.hpp>
+#include <moveit/robot_state/cartesian_interpolator.hpp>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>
 #else
diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp
index 8c092e0e3..3886c9093 100644
--- a/core/src/solvers/joint_interpolation.cpp
+++ b/core/src/solvers/joint_interpolation.cpp
@@ -37,8 +37,8 @@
 */
 
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/trajectory_processing/time_parameterization.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/trajectory_processing/time_parameterization.hpp>
 
 #include <chrono>
 
diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp
index 59b48d9af..6a7017831 100644
--- a/core/src/solvers/multi_planner.cpp
+++ b/core/src/solvers/multi_planner.cpp
@@ -37,7 +37,7 @@
 */
 
 #include <moveit/task_constructor/solvers/multi_planner.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
 #include <chrono>
 
 namespace moveit {
diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp
index e173e7ca7..b985a9917 100644
--- a/core/src/solvers/pipeline_planner.cpp
+++ b/core/src/solvers/pipeline_planner.cpp
@@ -38,10 +38,10 @@
 
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit/task_constructor/task.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/planning_pipeline/planning_pipeline.hpp>
 #include <moveit_msgs/msg/motion_plan_request.hpp>
-#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/kinematic_constraints/utils.hpp>
 
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>
diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp
index a58b110da..9163e97cd 100644
--- a/core/src/solvers/planner_interface.cpp
+++ b/core/src/solvers/planner_interface.cpp
@@ -37,7 +37,7 @@
 */
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
-#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
 
 using namespace trajectory_processing;
 
diff --git a/core/src/stage.cpp b/core/src/stage.cpp
index 97e519321..9dba29cd2 100644
--- a/core/src/stage.cpp
+++ b/core/src/stage.cpp
@@ -40,7 +40,7 @@
 #include <moveit/task_constructor/introspection.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <rclcpp/logging.hpp>
 
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
index 6644fc95f..839db52e8 100644
--- a/core/src/stages/compute_ik.cpp
+++ b/core/src/stages/compute_ik.cpp
@@ -39,9 +39,9 @@
 #include <moveit/task_constructor/marker_tools.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/conversions.h>
-#include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/conversions.hpp>
+#include <moveit/robot_state/robot_state.hpp>
 
 #include <Eigen/Geometry>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp
index 3e1923503..c5a5eab52 100644
--- a/core/src/stages/connect.cpp
+++ b/core/src/stages/connect.cpp
@@ -41,8 +41,8 @@
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
 
 #if FMT_VERSION >= 90000
 template <>
diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp
index 5a5d4c158..66b61368d 100644
--- a/core/src/stages/current_state.cpp
+++ b/core/src/stages/current_state.cpp
@@ -41,8 +41,8 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit_msgs/srv/get_planning_scene.hpp>
 #include <moveit_msgs/msg/planning_scene_components.hpp>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_model_loader/robot_model_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace moveit {
diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp
index 8b4042ca6..fe225eebc 100644
--- a/core/src/stages/fix_collision_objects.cpp
+++ b/core/src/stages/fix_collision_objects.cpp
@@ -40,7 +40,7 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <moveit/task_constructor/cost_terms.h>
 
 #include <rviz_marker_tools/marker_creation.h>
diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp
index 97ff83617..fe0653a1f 100644
--- a/core/src/stages/fixed_cartesian_poses.cpp
+++ b/core/src/stages/fixed_cartesian_poses.cpp
@@ -39,7 +39,7 @@
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/marker_tools.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_marker_tools/marker_creation.h>
 
 namespace moveit {
diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp
index 0c8ebaade..7a34645a9 100644
--- a/core/src/stages/fixed_state.cpp
+++ b/core/src/stages/fixed_state.cpp
@@ -37,7 +37,7 @@
 #include <moveit/task_constructor/stages/fixed_state.h>
 #include <moveit/task_constructor/cost_terms.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp
index 5a886e4c7..8ccc3b3bc 100644
--- a/core/src/stages/generate_grasp_pose.cpp
+++ b/core/src/stages/generate_grasp_pose.cpp
@@ -39,8 +39,8 @@
 #include <moveit/task_constructor/marker_tools.h>
 #include <rviz_marker_tools/marker_creation.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/conversions.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/conversions.hpp>
 
 #include <Eigen/Geometry>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp
index 42a3205ca..0566f07e6 100644
--- a/core/src/stages/generate_place_pose.cpp
+++ b/core/src/stages/generate_place_pose.cpp
@@ -40,9 +40,9 @@
 
 #include <rviz_marker_tools/marker_creation.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/robot_state.h>
-#include <moveit/robot_state/attached_body.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/robot_state.hpp>
+#include <moveit/robot_state/attached_body.hpp>
 
 #include <Eigen/Geometry>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp
index 9f00cc9bc..26ea74265 100644
--- a/core/src/stages/generate_pose.cpp
+++ b/core/src/stages/generate_pose.cpp
@@ -38,7 +38,7 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/marker_tools.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_marker_tools/marker_creation.h>
 
 namespace moveit {
diff --git a/core/src/stages/generate_random_pose.cpp b/core/src/stages/generate_random_pose.cpp
index 1ab6cfeef..ddacfebfc 100644
--- a/core/src/stages/generate_random_pose.cpp
+++ b/core/src/stages/generate_random_pose.cpp
@@ -37,7 +37,7 @@
 #include <moveit/task_constructor/stages/generate_random_pose.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/marker_tools.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_marker_tools/marker_creation.h>
 
 #include <Eigen/Geometry>
diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp
index 21b86a4e5..55feac1e8 100644
--- a/core/src/stages/modify_planning_scene.cpp
+++ b/core/src/stages/modify_planning_scene.cpp
@@ -41,7 +41,7 @@
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/fmt_p.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
index 46e4208bc..c0bf6d6d0 100644
--- a/core/src/stages/move_relative.cpp
+++ b/core/src/stages/move_relative.cpp
@@ -39,7 +39,7 @@
 #include <moveit/task_constructor/stages/move_relative.h>
 #include <moveit/task_constructor/cost_terms.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_marker_tools/marker_creation.h>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>
diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp
index 59f311e57..7399fcb43 100644
--- a/core/src/stages/move_to.cpp
+++ b/core/src/stages/move_to.cpp
@@ -36,8 +36,8 @@
    Desc:    Move to joint-state or Cartesian goal pose
 */
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_state/conversions.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_state/conversions.hpp>
 
 #include <moveit/task_constructor/stages/move_to.h>
 #include <moveit/task_constructor/cost_terms.h>
@@ -49,7 +49,7 @@
 #else
 #include <tf2_eigen/tf2_eigen.h>
 #endif
-#include <moveit/robot_state/conversions.h>
+#include <moveit/robot_state/conversions.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp
index 6a2e9987a..116f09f49 100644
--- a/core/src/stages/pick.cpp
+++ b/core/src/stages/pick.cpp
@@ -5,7 +5,7 @@
 #include <moveit/task_constructor/container.h>
 #include <moveit/task_constructor/stages/move_relative.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 namespace moveit {
 namespace task_constructor {
diff --git a/core/src/stages/predicate_filter.cpp b/core/src/stages/predicate_filter.cpp
index d64b2cb81..1f8a9fc00 100644
--- a/core/src/stages/predicate_filter.cpp
+++ b/core/src/stages/predicate_filter.cpp
@@ -37,10 +37,10 @@
 
 #include <moveit/task_constructor/storage.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
-#include <moveit/robot_state/conversions.h>
-#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_state/conversions.hpp>
+#include <moveit/robot_state/robot_state.hpp>
 
 #include <functional>
 
diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp
index edd189e5a..1cc5318d0 100644
--- a/core/src/stages/simple_grasp.cpp
+++ b/core/src/stages/simple_grasp.cpp
@@ -41,7 +41,7 @@
 #include <moveit/task_constructor/stages/move_to.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <Eigen/Geometry>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
diff --git a/core/src/storage.cpp b/core/src/storage.cpp
index b4ad597cc..3a54ba8df 100644
--- a/core/src/storage.cpp
+++ b/core/src/storage.cpp
@@ -38,9 +38,9 @@
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/introspection.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
-#include <moveit/robot_state/conversions.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/robot_state/conversions.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <assert.h>
 
 namespace moveit {
diff --git a/core/src/task.cpp b/core/src/task.cpp
index 9c31d5db9..f8853e70c 100644
--- a/core/src/task.cpp
+++ b/core/src/task.cpp
@@ -43,8 +43,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 
-#include <moveit/robot_model_loader/robot_model_loader.h>
-#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit/robot_model_loader/robot_model_loader.hpp>
+#include <moveit/planning_pipeline/planning_pipeline.hpp>
 
 #include <scope_guard/scope_guard.hpp>
 
diff --git a/core/src/utils.cpp b/core/src/utils.cpp
index d5ee5119a..63cef23e8 100644
--- a/core/src/utils.cpp
+++ b/core/src/utils.cpp
@@ -41,8 +41,8 @@
 #include <tf2_eigen/tf2_eigen.h>
 #endif
 
-#include <moveit/robot_state/robot_state.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/robot_state.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/storage.h>
diff --git a/core/test/models.cpp b/core/test/models.cpp
index 8ee16f792..5ea0844c1 100644
--- a/core/test/models.cpp
+++ b/core/test/models.cpp
@@ -1,6 +1,6 @@
 #include "models.h"
-#include <moveit/utils/robot_model_test_utils.h>
-#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/utils/robot_model_test_utils.hpp>
+#include <moveit/robot_model_loader/robot_model_loader.hpp>
 
 using namespace moveit::core;
 
diff --git a/core/test/models.h b/core/test/models.h
index 43fbd850a..8b98fd463 100644
--- a/core/test/models.h
+++ b/core/test/models.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace moveit {
diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp
index 4ee7b9336..937034d22 100644
--- a/core/test/pick_pa10.cpp
+++ b/core/test/pick_pa10.cpp
@@ -14,7 +14,7 @@
 #include <moveit/task_constructor/stages/fix_collision_objects.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <gtest/gtest.h>
 
 using namespace moveit::task_constructor;
diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp
index c8a31c792..0b15fc7e4 100644
--- a/core/test/pick_pr2.cpp
+++ b/core/test/pick_pr2.cpp
@@ -8,7 +8,7 @@
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
 #include <gtest/gtest.h>
 
 using namespace moveit::task_constructor;
diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp
index d268bdc15..377f63b59 100644
--- a/core/test/pick_ur5.cpp
+++ b/core/test/pick_ur5.cpp
@@ -8,7 +8,7 @@
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
 #include <gtest/gtest.h>
 
 using namespace moveit::task_constructor;
diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp
index 37be534c3..05f0c28f8 100644
--- a/core/test/stage_mockups.cpp
+++ b/core/test/stage_mockups.cpp
@@ -1,5 +1,5 @@
 #include <moveit/task_constructor/stage_p.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include "stage_mockups.h"
 
diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h
index 35d01e659..d17505d29 100644
--- a/core/test/stage_mockups.h
+++ b/core/test/stage_mockups.h
@@ -3,7 +3,7 @@
 #include <moveit/task_constructor/task.h>
 #include <moveit/task_constructor/cost_terms.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include "models.h"
 
diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp
index 5d7b722c9..aecd598f3 100644
--- a/core/test/test_container.cpp
+++ b/core/test/test_container.cpp
@@ -2,7 +2,7 @@
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/task_p.h>
 #include <moveit/task_constructor/stages/fixed_state.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include "stage_mockups.h"
 #include "models.h"
diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp
index 8b49c8c41..6c6264f63 100644
--- a/core/test/test_cost_terms.cpp
+++ b/core/test/test_cost_terms.cpp
@@ -5,7 +5,7 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/stages/passthrough.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp
index 49dbc69c8..03ad7580c 100644
--- a/core/test/test_fallback.cpp
+++ b/core/test/test_fallback.cpp
@@ -3,7 +3,7 @@
 #include <moveit/task_constructor/task_p.h>
 #include <moveit/task_constructor/stages/predicate_filter.h>
 #include <moveit/task_constructor/stages/noop.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include "stage_mockups.h"
 #include "models.h"
diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp
index 65aeee36f..fa3d56bee 100644
--- a/core/test/test_interface_state.cpp
+++ b/core/test/test_interface_state.cpp
@@ -1,6 +1,6 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/stage_p.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include "models.h"
 #include <memory>
diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp
index 80a0ee810..cf60bab2d 100644
--- a/core/test/test_move_relative.cpp
+++ b/core/test/test_move_relative.cpp
@@ -6,8 +6,8 @@
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp
index 994b23602..68ebacc86 100644
--- a/core/test/test_move_to.cpp
+++ b/core/test/test_move_to.cpp
@@ -8,7 +8,7 @@
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>
diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp
index 43df3153a..7ae0209c6 100644
--- a/core/test/test_pipeline_planner.cpp
+++ b/core/test/test_pipeline_planner.cpp
@@ -1,7 +1,7 @@
 #include "models.h"
 
 #include <gtest/gtest.h>
-#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model/robot_model.hpp>
 
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 
diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp
index ded84eb95..bdb8ff5ba 100644
--- a/core/test/test_serial.cpp
+++ b/core/test/test_serial.cpp
@@ -2,8 +2,8 @@
 #include <moveit/task_constructor/stages.h>
 #include <moveit/task_constructor/cost_terms.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/utils/robot_model_test_utils.hpp>
 
 #include "stage_mockups.h"
 #include "models.h"
diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp
index e6a072593..1c0553cee 100644
--- a/core/test/test_stage.cpp
+++ b/core/test/test_stage.cpp
@@ -3,7 +3,7 @@
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/stages/compute_ik.h>
 #include <moveit/task_constructor/stages/modify_planning_scene.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include "stage_mockups.h"
diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h
index da76c2fae..2d0d5bd07 100644
--- a/demo/include/moveit_task_constructor_demo/pick_place_task.h
+++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h
@@ -38,9 +38,9 @@
 #include <rclcpp/node.hpp>
 
 // MoveIt
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_model/robot_model.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
 
 // MTC
 #include <moveit/task_constructor/task.h>
diff --git a/demo/src/alternative_path_costs.cpp b/demo/src/alternative_path_costs.cpp
index f9c302312..dfab41041 100644
--- a/demo/src/alternative_path_costs.cpp
+++ b/demo/src/alternative_path_costs.cpp
@@ -1,6 +1,6 @@
 #include <rclcpp/rclcpp.hpp>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <moveit/task_constructor/task.h>
 #include <moveit/task_constructor/container.h>
diff --git a/demo/src/cartesian.cpp b/demo/src/cartesian.cpp
index bf7a1dbec..42524e67d 100644
--- a/demo/src/cartesian.cpp
+++ b/demo/src/cartesian.cpp
@@ -42,7 +42,7 @@
 #include <moveit/task_constructor/stages/connect.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 using namespace moveit::task_constructor;
 
diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp
index 929dedd7b..212f19c16 100644
--- a/demo/src/fallbacks_move_to.cpp
+++ b/demo/src/fallbacks_move_to.cpp
@@ -1,7 +1,7 @@
 #include <rclcpp/rclcpp.hpp>
 
-#include <moveit/robot_model/robot_model.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <moveit/task_constructor/task.h>
 #include <moveit/task_constructor/container.h>
diff --git a/demo/src/ik_clearance_cost.cpp b/demo/src/ik_clearance_cost.cpp
index 4ef5bffd3..abc4c8fdd 100644
--- a/demo/src/ik_clearance_cost.cpp
+++ b/demo/src/ik_clearance_cost.cpp
@@ -1,6 +1,6 @@
 #include <rclcpp/rclcpp.hpp>
 
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <moveit/task_constructor/task.h>
 
diff --git a/demo/src/modular.cpp b/demo/src/modular.cpp
index d841f2b96..2eb016f96 100644
--- a/demo/src/modular.cpp
+++ b/demo/src/modular.cpp
@@ -43,7 +43,7 @@
 #include <moveit/task_constructor/container.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 using namespace moveit::task_constructor;
 
diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.h b/visualization/motion_planning_tasks/src/meta_task_list_model.h
index 2c87b8474..943f9df34 100644
--- a/visualization/motion_planning_tasks/src/meta_task_list_model.h
+++ b/visualization/motion_planning_tasks/src/meta_task_list_model.h
@@ -37,7 +37,7 @@
 #pragma once
 
 #include <utils/tree_merge_proxy_model.h>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <QVector>
 
 namespace moveit_rviz_plugin {
diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp
index df63a2164..03f3cfac6 100644
--- a/visualization/motion_planning_tasks/src/remote_task_model.cpp
+++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp
@@ -40,7 +40,7 @@
 #include "properties/property_factory.h"
 #include <moveit/task_constructor/container.h>
 #include <moveit/task_constructor/properties.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 #include <rviz_common/properties/property_tree_model.hpp>
 #include <rviz_common/properties/string_property.hpp>
 #include <rclcpp/logging.hpp>
diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp
index 582ec462b..0c07a2dea 100644
--- a/visualization/motion_planning_tasks/src/task_display.cpp
+++ b/visualization/motion_planning_tasks/src/task_display.cpp
@@ -46,8 +46,8 @@
 #include <moveit/visualization_tools/display_solution.h>
 #include <moveit_task_constructor_msgs/srv/get_solution.hpp>
 
-#include <moveit/rdf_loader/rdf_loader.h>
-#include <moveit/robot_model/robot_model.h>
+#include <moveit/rdf_loader/rdf_loader.hpp>
+#include <moveit/robot_model/robot_model.hpp>
 
 #include <rviz_common/display_context.hpp>
 #include <rviz_common/properties/string_property.hpp>
diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h
index e1b5015fc..41fa159d1 100644
--- a/visualization/motion_planning_tasks/src/task_display.h
+++ b/visualization/motion_planning_tasks/src/task_display.h
@@ -45,7 +45,7 @@
 #ifndef Q_MOC_RUN
 #include "job_queue.h"
 #include "local_task_model.h"
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <rclcpp/subscription.hpp>
 #include <moveit_task_constructor_msgs/msg/task_description.hpp>
 #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h
index b6e6d6c1b..c53de7dbf 100644
--- a/visualization/motion_planning_tasks/src/task_list_model.h
+++ b/visualization/motion_planning_tasks/src/task_list_model.h
@@ -41,7 +41,7 @@
 #include <moveit/task_constructor/stage.h>
 #include <utils/flat_merge_proxy_model.h>
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <rclcpp/node.hpp>
 #include <moveit_task_constructor_msgs/msg/task_description.hpp>
 #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h
index 68c6462d9..59c0eed8c 100644
--- a/visualization/motion_planning_tasks/src/task_panel.h
+++ b/visualization/motion_planning_tasks/src/task_panel.h
@@ -39,7 +39,7 @@
 #pragma once
 
 #include <rviz_common/panel.hpp>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <QModelIndex>
 class QItemSelection;
 class QIcon;
diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h
index fae2d866b..7f8cd2980 100644
--- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h
+++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h
@@ -37,7 +37,7 @@
 #pragma once
 
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 
 namespace moveit {
 namespace core {
diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h
index 1db0d028e..1b3fcebe3 100644
--- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h
+++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <rviz_common/properties/bool_property.hpp>
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <deque>
 #include <list>
diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h
index 101903cef..c1a666b73 100644
--- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h
+++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h
@@ -36,7 +36,7 @@
 
 #pragma once
 
-#include <moveit/macros/class_forward.h>
+#include <moveit/macros/class_forward.hpp>
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
 #include <QObject>
 #include <boost/thread/mutex.hpp>
diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp
index 305cb32b1..b9f2994f5 100644
--- a/visualization/visualization_tools/src/display_solution.cpp
+++ b/visualization/visualization_tools/src/display_solution.cpp
@@ -36,8 +36,8 @@
 
 #include <moveit/visualization_tools/display_solution.h>
 #include <moveit/visualization_tools/marker_visualization.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
 #include <rclcpp/logging.hpp>
 #include <fmt/core.h>
 
diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp
index 82bdd7836..031dd48d0 100644
--- a/visualization/visualization_tools/src/marker_visualization.cpp
+++ b/visualization/visualization_tools/src/marker_visualization.cpp
@@ -1,5 +1,5 @@
 #include <moveit/visualization_tools/marker_visualization.h>
-#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene/planning_scene.hpp>
 
 #include <rviz_default_plugins/displays/marker/markers/marker_base.hpp>
 #include <rviz_default_plugins/displays/marker/markers/marker_factory.hpp>
diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp
index b53d4b4f8..c65dab9b4 100644
--- a/visualization/visualization_tools/src/task_solution_visualization.cpp
+++ b/visualization/visualization_tools/src/task_solution_visualization.cpp
@@ -39,13 +39,13 @@
 #include <moveit/visualization_tools/marker_visualization.h>
 #include <moveit/visualization_tools/task_solution_panel.h>
 
-#include <moveit/rviz_plugin_render_tools/planning_scene_render.h>
-#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
-#include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
+#include <moveit/rviz_plugin_render_tools/planning_scene_render.hpp>
+#include <moveit/rviz_plugin_render_tools/robot_state_visualization.hpp>
+#include <moveit/rviz_plugin_render_tools/planning_link_updater.hpp>
 
-#include <moveit/robot_model/robot_model.h>
-#include <moveit/robot_trajectory/robot_trajectory.h>
-#include <moveit/trajectory_processing/trajectory_tools.h>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/trajectory_processing/trajectory_tools.hpp>
 
 #include <rviz_default_plugins/robot/robot.hpp>
 #include <rviz_default_plugins/robot/robot_link.hpp>