diff --git a/CMakeLists.txt b/CMakeLists.txt
index e6ac4627..2bce9fa5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,11 @@ find_package(RTABMap 0.20.5 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
 find_package(PCL 1.7 REQUIRED COMPONENTS kdtree) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908
 
+# optional
+find_package(rviz_common)
+find_package(rviz_rendering)
+find_package(rviz_default_plugins)
+
 #Qt stuff
 # If librtabmap_gui.so is found, rtabmapviz will be built
 # If rviz is found, plugins will be built
@@ -79,11 +84,6 @@ IF(RTABMAP_GUI OR rviz_default_plugins_FOUND)
    ENDIF()   
 ENDIF(RTABMAP_GUI OR rviz_default_plugins_FOUND)
 
-find_package(rviz_common REQUIRED)
-find_package(rviz_rendering REQUIRED)
-find_package(rviz_default_plugins REQUIRED)
-
-
 #######################################
 ## Declare ROS messages and services ##
 #######################################
@@ -450,9 +450,11 @@ foreach(typesupport_impl ${typesupport_impls})
   rosidl_target_interfaces(rtabmap
     ${PROJECT_NAME}_msgs ${typesupport_impl}
   )
-  rosidl_target_interfaces(rtabmapviz
-    ${PROJECT_NAME}_msgs ${typesupport_impl}
-  )
+  IF(RTABMAP_GUI)
+    rosidl_target_interfaces(rtabmapviz
+      ${PROJECT_NAME}_msgs ${typesupport_impl}
+    )
+  ENDIF()
 endforeach()
 
 # If rviz is found, add plugins