diff --git a/CMakeLists.txt b/CMakeLists.txt
index d75ed4bb..36b1f34b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -205,10 +205,12 @@ ament_export_dependencies(rosidl_default_runtime)
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
+#include_directories(include)
 include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}/include
   ${RTABMap_INCLUDE_DIRS}
+)
+include_directories(SYSTEM
   ${PCL_INCLUDE_DIRS}
 )
 
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 4948ec22..3d65a528 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -44,8 +44,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <geometry_msgs/msg/transform.hpp>
 #include <laser_geometry/laser_geometry.hpp>
 #include <rtabmap/core/util3d_surface.h>
-#include <tf2_eigen/tf2_eigen.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace rtabmap_ros {