diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6e6ea89902bb0aaedff848c641693837bbf881a4..be57aab126864ef4eebfb3cb64fd1e0596fc4062 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,9 @@ find_package(pal_waypoint_msgs)
 
 if(${pal_waypoint_msgs_FOUND})
   ADD_DEFINITIONS(-DHAVE_WAYPOINTS)
+  set(pal_waypoint_msgs_catkin_depends pal_navigation_msgs)
+else()
+  set(pal_waypoint_msgs_catkin_depends )
 endif()
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -109,7 +112,7 @@ set(module_bt_name tiago_nav_module_bt)
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs iri_base_algorithm iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs ${pal_waypoint_msgs_catkin_depends} iri_base_algorithm iri_behaviortree
 #  DEPENDS system_lib
 )