diff --git a/CMakeLists.txt b/CMakeLists.txt
index 065815a918a209f558e1b029fb83e5c247b049cf..072f313281ea11cf204a65841314be0d68b27ad0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,7 +15,8 @@ find_package(catkin REQUIRED
         moveit_core
         moveit_ros_planning_interface
         franka_gripper
-        message_generation)
+        message_generation
+        )
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -40,6 +41,8 @@ catkin_package(CATKIN_DEPENDS
         moveit_visual_tools
         moveit_ros_planning_interface
         pluginlib
+        INCLUDE_DIRS include
+        LIBRARIES EnvironmentUtil GraspUtil FrankaGripperUtil
         )
 
 # ################################################################################################################################
@@ -47,11 +50,16 @@ catkin_package(CATKIN_DEPENDS
 # ################################################################################################################################
 
 # Specify additional locations of header files Your package locations should be listed before other locations
-include_directories(${catkin_INCLUDE_DIRS})
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+)
 
-add_library(EnvironmentUtil src/grasping/environment_util.cpp src/grasping/environment_util.h)
-add_library(GraspUtil src/grasping/grasp_util.cpp src/grasping/grasp_util.h)
-add_library(FrankaGripperUtil src/grasping/franka_gripper_util.cpp src/grasping/franka_gripper_util.h)
+add_library(EnvironmentUtil src/grasping/environment_util.cpp)
+add_library(GraspUtil src/grasping/grasp_util.cpp)
+add_library(FrankaGripperUtil src/grasping/franka_gripper_util.cpp)
 
 add_executable(grasp_service src/grasping/grasp_service.cpp)
 
diff --git a/src/grasping/environment_util.h b/include/environment_util.h
similarity index 100%
rename from src/grasping/environment_util.h
rename to include/environment_util.h
diff --git a/src/grasping/franka_gripper_util.h b/include/franka_gripper_util.h
similarity index 100%
rename from src/grasping/franka_gripper_util.h
rename to include/franka_gripper_util.h
diff --git a/src/grasping/grasp_util.h b/include/grasp_util.h
similarity index 100%
rename from src/grasping/grasp_util.h
rename to include/grasp_util.h