diff --git a/CMakeLists.txt b/CMakeLists.txt
index db75b43b88ef363e7e072c810bfa0d2832f57a5a..70e242940617ed3dd6a4502c801341b682d5984f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,10 +17,39 @@
 
 # Catkin part
 
-
 cmake_minimum_required(VERSION 2.4.6)
 
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0003 NEW)
+endif(COMMAND cmake_policy)
+
+SET(PROJECT_ORG stack-of-tasks)
+set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
+set(PROJECT_NAME dynamic_graph_bridge)
+set(PROJECT_URL "https://github.com/stack-of-tasks/dynamic_graph_bridge")
+set(PROJECT_SUFFIX "-v3")
+
+# Export CMake Target
+SET(PROJECT_USE_CMAKE_EXPORT TRUE)
+
+# Disable failing compilation when a compilation error appears
+set(CXX_DISABLE_WERROR False)
+
+# Make sure that every header is generated in dynamic-graph
+SET(CUSTOM_HEADER_DIR ${PROJECT_NAME})
+
 include(cmake/base.cmake)
+
+# Specify the project.
+cmake_policy(SET CMP0048 NEW)
+PROJECT(${PROJECT_NAME}
+  LANGUAGES
+  CXX
+  VERSION
+  ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}
+  )
+
+
 INCLUDE(cmake/boost.cmake)
 INCLUDE(cmake/eigen.cmake)
 include(cmake/ros.cmake)
@@ -28,35 +57,18 @@ include(cmake/GNUInstallDirs.cmake)
 include(cmake/python.cmake)
 include(cmake/test.cmake)
 
-project(dynamic_graph_bridge)
-
-SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf)
+SET(CATKIN_REQUIRED_COMPONENTS
+  roscpp std_msgs message_generation std_srvs geometry_msgs sensor_msgs
+  tf tf2_bullet)
 SET(CATKIN_DEPENDS_LIBRARIES ros_bridge sot_loader)
 
 ## LAAS cmake submodule part
-set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
-set(PROJECT_NAME dynamic_graph_bridge)
-set(PROJECT_URL "https://github.com/stack-of-tasks/dynamic_graph_bridge")
-set(PROJECT_SUFFIX "-v3")
 
 SET(DOXYGEN_USE_MATHJAX YES)
 
+# Add option build python interface
 OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
-IF(BUILD_PYTHON_INTERFACE)
-  FINDPYTHON()
-  STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
-  INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
-  ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
-  SET(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
-  SET(CATKIN_DEPENDS_LIBRARIES ${CATKIN_DEPENDS_LIBRARIES} ros_interpreter)
-ENDIF(BUILD_PYTHON_INTERFACE)
-
-SET(CATKIN_ENABLE_TESTING OFF)
-
-find_package(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
-find_package(realtime_tools)
 
-set(CXX_DISABLE_WERROR False)
 set(CUSTOM_HEADER_DIR dynamic_graph_bridge)
 set(${PROJECT_NAME}_HEADERS
   include/dynamic_graph_bridge/ros_init.hh
@@ -69,10 +81,28 @@ IF(BUILD_PYTHON_INTERFACE)
     include/dynamic_graph_bridge/ros_interpreter.hh )
 ENDIF(BUILD_PYTHON_INTERFACE)
 
+IF(BUILD_PYTHON_INTERFACE)
+  FINDPYTHON()
+  STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+  
+  ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED )
+  SET(CATKIN_REQUIRED_COMPONENTS ${CATKIN_REQUIRED_COMPONENTS} rospy)
+  SET(CATKIN_DEPENDS_LIBRARIES ${CATKIN_DEPENDS_LIBRARIES} ros_interpreter)
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+SET(CATKIN_ENABLE_TESTING OFF)
+
+MESSAGE(STATUS "CATKIN_REQUIRED_COMPONENTS: ${CATKIN_REQUIRED_COMPONENTS}")
+# Add catkin components
+find_package(catkin REQUIRED
+  COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
+find_package(realtime_tools)
+
+
 SEARCH_FOR_EIGEN()
 SEARCH_FOR_BOOST()
 
-SETUP_PROJECT()
+SETUP_PROJECT_PACKAGE_FINALIZE()
 
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
@@ -88,15 +118,13 @@ set(PKG_CONFIG_ADDITIONAL_VARIABLES
 SET(SOT_PKGNAMES
 dynamic_graph_bridge_msgs)
 
-add_required_dependency(roscpp)
-add_required_dependency(tf)
-add_required_dependency("realtime_tools >= 1.8")
-add_required_dependency(tf2_bullet)
+#add_project_dependency(realtime_tools 1.8 REQUIRED)
+
 
-ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
-ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
+ADD_PROJECT_DEPENDENCY(dynamic-graph  3.0.0 REQUIRED)
+ADD_PROJECT_DEPENDENCY(sot-core REQUIRED)
 
-add_required_dependency(dynamic_graph_bridge_msgs)
+ADD_PROJECT_dependency(dynamic_graph_bridge_msgs)
 
 foreach(sot_pkgname ${SOT_PKGNAMES})
   add_required_dependency(${sot_pkgname})
@@ -110,13 +138,36 @@ add_library(ros_bridge
   include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
   src/sot_to_ros.hh src/sot_to_ros.cpp
   )
-pkg_config_use_dependency(ros_bridge tf2_bullet)
-pkg_config_use_dependency(ros_bridge dynamic_graph_bridge_msgs)
-install(TARGETS ros_bridge DESTINATION lib)
 
-# Add ros_bridge in the dynamic-graph-bridge pkg-config file.
+MESSAGE(STATUS "catkin_INCLUDE_DIRS: ${catkin_INCLUDE_DIRS}")
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+target_include_directories(ros_bridge
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_BUILD_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/include>
+  $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+  $<BUILD_INTERFACE:${PYTHON_INCLUDE_DIRS}>
+  INTERFACE ${EIGEN3_INCLUDE_DIR}
+  $<INSTALL_INTERFACE:include>
+  )
 
+#target_link_libraries(ros_bridge tf2_bullet)
+target_link_libraries(ros_bridge
+  dynamic-graph::dynamic-graph)
+target_link_libraries(ros_bridge
+  dynamic-graph::dynamic-graph)
 
+MESSAGE(STATUS "catkin_LIBRARIES: ${catkin_LIBRARIES}")
+target_link_libraries(ros_bridge
+  ${catkin_LIBRARIES} )
+
+install(TARGETS ros_bridge
+  EXPORT ${TARGETS_EXPORT_NAME}  
+  DESTINATION lib)
+
+# Add ros_bridge in the dynamic-graph-bridge pkg-config file.
 # Make sure rpath are preserved during the install as ROS dependencies
 # are not installed.
 set_target_properties(ros_bridge PROPERTIES BUILD_WITH_INSTALL_RPATH True
@@ -126,9 +177,22 @@ macro(compile_plugin NAME)
   message(lib path ${LIBRARY_OUTPUT_PATH})
   file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}")
   add_library(${NAME} SHARED src/${NAME}.cpp src/${NAME}.hh)
-  pkg_config_use_dependency(${NAME} dynamic-graph)
-  pkg_config_use_dependency(${NAME} sot-core)
-  pkg_config_use_dependency(${NAME} dynamic_graph_bridge_msgs)
+
+  # Headers
+  target_include_directories(${NAME}
+    PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_BUILD_DIR}/include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/include>
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+    $<BUILD_INTERFACE:${PYTHON_INCLUDE_DIRS}>
+    INTERFACE ${EIGEN3_INCLUDE_DIR}
+    $<INSTALL_INTERFACE:include>
+    )
+
+  # Libraries
+  target_link_libraries(${NAME} dynamic-graph::dynamic-graph)
+  target_link_libraries(${NAME} sot-core::sot-core)
+  target_link_libraries(${NAME} ${CATKIN_DEPENDS_LIBRARIES})
   add_dependencies(${NAME} ros_bridge)
   target_link_libraries(${NAME} ros_bridge)
   set_target_properties(${NAME} PROPERTIES BUILD_WITH_INSTALL_RPATH True)
@@ -137,7 +201,8 @@ macro(compile_plugin NAME)
 endmacro()
 
 # Build Sot Entities
-set(listplugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener ros_time)
+set(listplugins
+  ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener ros_time)
 
 foreach(aplugin ${listplugins})
   compile_plugin(${aplugin})
@@ -152,22 +217,29 @@ IF(BUILD_PYTHON_INTERFACE)
       ros/${NAME}/wrap
       )
 
-    PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap realtime_tools)
-    PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph)
-    PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
+    #PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap realtime_tools)
+    #PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph)
+    #PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
     PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph_bridge_msgs)
   endforeach()
 
   # ros_interperter library.
 
   add_library(ros_interpreter src/ros_interpreter.cpp)
-  pkg_config_use_dependency(ros_interpreter dynamic-graph)
-  pkg_config_use_dependency(ros_interpreter sot-core)
-  pkg_config_use_dependency(ros_interpreter roscpp)
+  #pkg_config_use_dependency(ros_interpreter dynamic-graph)
+  #pkg_config_use_dependency(ros_interpreter sot-core)
+  #pkg_config_use_dependency(ros_interpreter roscpp)
   pkg_config_use_dependency(ros_interpreter dynamic_graph_bridge_msgs)
-  pkg_config_use_dependency(ros_interpreter dynamic-graph-python)
+  #pkg_config_use_dependency(ros_interpreter dynamic-graph-python)
 
   add_dependencies(ros_interpreter ros_bridge)
+  target_link_libraries(ros_interpreter roscpp)
+  target_link_libraries(ros_interpreter
+    dynamic-graph::dynamic-graph)
+  target_link_libraries(ros_interpreter sot-core::sot-core)
+  target_link_libraries(ros_interpreter
+    dynamic-graph-python::dynamic-graph-python)  
+  target_link_libraries(ros_interpreter realtime_tools)
   target_link_libraries(ros_interpreter ros_bridge)
   set_target_properties(ros_interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True
                         LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib)
@@ -176,18 +248,21 @@ IF(BUILD_PYTHON_INTERFACE)
 
 ENDIF(BUILD_PYTHON_INTERFACE)
 
+MESSAGE(STATUS "CATKIN_DEPENDS_LIBRARIES: ${CATKIN_DEPENDS_LIBRARIES}")
+
 # Stand alone embedded intepreter with a robot controller.
-add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp src/sot_loader_basic.cpp)
-pkg_config_use_dependency(geometric_simu tf)
-pkg_config_use_dependency(geometric_simu roscpp)
-pkg_config_use_dependency(geometric_simu dynamic-graph)
-target_link_libraries(geometric_simu  ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
+add_executable(geometric_simu src/geometric_simu.cpp
+  src/sot_loader.cpp src/sot_loader_basic.cpp)
+target_link_libraries(geometric_simu dynamic-graph::dynamic-graph)
+target_link_libraries(geometric_simu ros_bridge tf ${Boost_LIBRARIES}
+  ${CMAKE_DL_LIBS})
+target_link_libraries(geometric_simu ${CATKIN_DEPENDS_LIBRARIES})
 
 # Sot loader library
 add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
-pkg_config_use_dependency(sot_loader dynamic-graph)
-pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp ros_bridge tf)
+target_link_libraries(sot_loader dynamic-graph::dynamic-graph)
+target_link_libraries(sot_loader sot-core::sot-core)
+target_link_libraries(sot_loader ${Boost_LIBRARIES} )
 install(TARGETS sot_loader DESTINATION lib)
 
 add_subdirectory(src)
@@ -197,16 +272,18 @@ add_subdirectory(tests)
 add_service_files( FILES RunPythonFile.srv )
 generate_messages( DEPENDENCIES std_msgs )
 
-
-# This is necessary so that the pc file generated by catking is similar to the on
-# done directly by jrl-cmake-modules
-catkin_package(CATKIN_DEPENDS message_runtime roscpp realtime_tools tf2_bullet ${SOT_PKGNAMES} tf
+# This is necessary so that the pc file generated by catking is similar to
+# the on done directly by jrl-cmake-modules
+catkin_package(INCLUDE_DIRS include
+  CATKIN_DEPENDS
+  message_runtime roscpp realtime_tools tf2_bullet ${SOT_PKGNAMES} tf
   LIBRARIES ${CATKIN_DEPENDS_LIBRARIES}
 )
 
 # Add libraries in pc file generated by cmake submodule
 PKG_CONFIG_APPEND_LIBS(ros_bridge sot_loader)
 
+# In the python interface needs to be build.
 IF(BUILD_PYTHON_INTERFACE)
   PKG_CONFIG_APPEND_LIBS(ros_interpreter)
 
@@ -219,13 +296,24 @@ IF(BUILD_PYTHON_INTERFACE)
     )
 
   # Service file.
-  install(FILES ./srv/RunPythonFile.srv DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/srv)
+  install(FILES ./srv/RunPythonFile.srv
+    DESTINATION
+    ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/srv)
 
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 message(cmake_install_bindir " is ${CMAKE_INSTALL_BINDIR} ")
-install(TARGETS geometric_simu DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(FILES manifest.xml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/)
+
+# Install the geometrical simulation node
+install(TARGETS geometric_simu
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION
+  ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# Install package information
+install(FILES manifest.xml
+  DESTINATION
+  ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/)
 
 
-SETUP_PROJECT_FINALIZE()
+SETUP_PROJECT_PACKAGE_FINALIZE()
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/include/dynamic_graph_bridge/ros_init.hh
index a4d1dd46bff1abb8affd7dc34ee3b83d2efe58fd..75f73fb23bbfd280d43d41dec9821bea4999a124 100644
--- a/include/dynamic_graph_bridge/ros_init.hh
+++ b/include/dynamic_graph_bridge/ros_init.hh
@@ -1,6 +1,10 @@
 #ifndef ROS_INIT_HH
 #define ROS_INIT_HH
+
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include <ros/ros.h>
+#pragma GCC diagnostic pop
 
 namespace dynamicgraph {
 ros::NodeHandle& rosInit(bool createAsyncSpinner = false,
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index e70ac484a806959e9432c1473603aef78058b0e0..4616bf9a5acb393da9c07cfdc78ac5a85515384a 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -36,7 +36,11 @@
 #include <boost/thread/thread.hpp>
 
 // ROS includes
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include "ros/ros.h"
+#pragma GCC diagnostic pop
+
 #include "std_srvs/Empty.h"
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_broadcaster.h>
diff --git a/package.xml b/package.xml
index 0dfcefc5c265be0a223c5960463b521ff9292080..4b79ba18226135557ab58c147fbbf02c49d7fbb0 100644
--- a/package.xml
+++ b/package.xml
@@ -28,9 +28,9 @@
   <build_depend>message_generation</build_depend>
   <build_depend>message_runtime</build_depend>
   <build_depend>tf2_bullet</build_depend>
-  <build_depend>dynamic-graph</build_depend>
-  <build_depend>dynamic-graph-python</build_depend>
-  <build_depend>sot-core</build_depend>
+  <build_depend>dynamic_graph</build_depend>
+  <build_depend>dynamic_graph_python</build_depend>
+  <build_depend>sot_core</build_depend>
   <build_depend>sot-dynamic-pinocchio</build_depend>
   <build_depend>dynamic_graph_bridge_msgs</build_depend>
 
@@ -44,8 +44,8 @@
   <exec_depend>message_generation</exec_depend>
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>tf2_bullet</exec_depend>
-  <exec_depend>dynamic-graph</exec_depend>
-  <exec_depend>dynamic-graph-python</exec_depend>
+  <exec_depend>dynamic_graph</exec_depend>
+  <exec_depend>dynamic_graph_python</exec_depend>
   <exec_depend>sot-core</exec_depend>
   <exec_depend>sot-dynamic-pinocchio</exec_depend>
   <exec_depend>dynamic_graph_bridge_msgs</exec_depend>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 85861d397e2fd9390d0b8b93a31257f7febed691..5ed0c9d3f7730ad10e3339a140c03f4c1f6eaf25 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -6,3 +6,4 @@ IF(BUILD_PYTHON_INTERFACE)
     DESTINATION "${PYTHON_SITELIB}/dynamic_graph/ros"
     )
 ENDIF(BUILD_PYTHON_INTERFACE)
+
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index 23ec2978fcf6ec02169adebcf8db3aa766a295e4..859ff81b28b748f3b3102a7f0c5098a44198dea3 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -6,8 +6,10 @@
 #include <pinocchio/multibody/model.hpp>
 
 #include "dynamic_graph_bridge/ros_init.hh"
-
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include <ros/package.h>
+#pragma GCC diagnostic pop
 
 namespace dynamicgraph {
 
diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp
index 997a5941fee3b2a39f951e63fac51578bf76148c..3feedfb04faa78d3a735c9a779f21c62df04510f 100644
--- a/src/ros_queued_subscribe.cpp
+++ b/src/ros_queued_subscribe.cpp
@@ -23,9 +23,6 @@
 #include <boost/function.hpp>
 #include <boost/make_shared.hpp>
 
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
 
 #include <dynamic-graph/factory.h>
 
diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh
index 7fb3345aa19b7af03d1669fff0f225f434b965bb..2273a561cdbe1fdb3afc8d23cc02c3aafc647c04 100644
--- a/src/ros_queued_subscribe.hh
+++ b/src/ros_queued_subscribe.hh
@@ -30,8 +30,14 @@
 #include <dynamic-graph/command.h>
 #include <sot/core/matrix-geometry.hh>
 
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include <ros/ros.h>
 
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
+#pragma GCC diagnostic pop
+
 #include "converter.hh"
 #include "sot_to_ros.hh"
 
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 113ec2d4906ddbba53c607912e1a1b05233a56ea..aabaff27921c612ba9d318e7066a0f1f1e2f38c5 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -4,10 +4,6 @@
 #include <boost/function.hpp>
 #include <boost/make_shared.hpp>
 
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
 #include <dynamic-graph/factory.h>
 
 #include "dynamic_graph_bridge/ros_init.hh"
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 35b9e59b844a7527fb267d06f34d4657949adb98..4155b580a533b72187ef1e259c5db93f1d9ee201 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -10,8 +10,14 @@
 #include <dynamic-graph/command.h>
 #include <sot/core/matrix-geometry.hh>
 
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include <ros/ros.h>
 
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
+#pragma GCC diagnostic pop
+
 #include "converter.hh"
 #include "sot_to_ros.hh"
 
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 6a3de0bb8e65a7c99d329f33a8481b7914827973..6de09444d762f459d97e5ef0e407f453385f4bf8 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -5,6 +5,8 @@
 
 #include <boost/format.hpp>
 
+#pragma GCC diagnostic push
+#pragma GCC system_header
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/UInt32.h>
@@ -18,6 +20,7 @@
 #include "geometry_msgs/Twist.h"
 #include "geometry_msgs/TwistStamped.h"
 #include "geometry_msgs/Vector3Stamped.h"
+#pragma GCC diagnostic pop
 
 #include <dynamic-graph/signal-time-dependent.h>
 #include <dynamic-graph/linear-algebra.h>