diff --git a/CMakeLists.txt b/CMakeLists.txt
index c78ac99e2a58ab83354580ec71f221f8f92cb0ff..a8056ba7beb9765398e2496705a0298d0549ad78 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ project(tiago_modules)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -88,9 +88,10 @@ find_package(iriutils REQUIRED)
 generate_dynamic_reconfigure_options(
   cfg/GripperModule.cfg
   cfg/TorsoModule.cfg
-#  cfg/LedsModule.cfg
+  cfg/LedsModule.cfg
   cfg/PlayMotionModule.cfg
   cfg/HeadModule.cfg
+  cfg/NavModule.cfg
 )
 
 ###################################
@@ -104,8 +105,8 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES gripper_module play_motion_module head_module torso_module #leds_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs
+ LIBRARIES gripper_module play_motion_module head_module torso_module nav_module #leds_module
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs
 #  DEPENDS system_lib
 )
 
@@ -195,6 +196,21 @@ target_link_libraries(head_module ${iriutils_LIBRARY})
 ## either from message generation or dynamic reconfigure
 add_dependencies(head_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+## Declare a C++ library
+add_library(nav_module
+  src/nav_module.cpp
+)
+
+target_link_libraries(nav_module ${catkin_LIBRARIES})
+target_link_libraries(nav_module ${iriutils_LIBRARY})
+##Link to other modules
+##target_link_libraries(new_module <module path>/lib<module_name>.so)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(nav_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
diff --git a/package.xml b/package.xml
index d1a150957eebfd2304729a1c01b7b6f4bebc074c..15f3a223ae018dc2a6877de684b0ecc2550ff6ca 100644
--- a/package.xml
+++ b/package.xml
@@ -48,6 +48,11 @@
   <build_depend>play_motion_msgs</build_depend>
   <build_depend>control_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>pal_navigation_msgs</build_depend>
+  <build_depend>pal_waypoint_msgs</build_depend>
+  <build_depend>tf</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
   <run_depend>iri_ros_tools</run_depend>
@@ -58,6 +63,11 @@
   <run_depend>play_motion_msgs</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>move_base_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>pal_navigation_msgs</run_depend>
+  <run_depend>pal_waypoint_msgs</run_depend>
+  <run_depend>tf</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->