diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt
index 6e1bbb3..491f965 100755
--- a/robotis_controller/CMakeLists.txt
+++ b/robotis_controller/CMakeLists.txt
@@ -19,6 +19,10 @@ find_package(catkin REQUIRED COMPONENTS
   cmake_modules
 )
 
+set(ENV{PKG_CONFIG_PATH} "$ENV{PKG_CONFIG_PATH}:${CATKIN_DEVEL_PREFIX}/lib/pkgconfig")
+find_package(PkgConfig)
+pkg_check_modules(yaml_cpp yaml-cpp REQUIRED)
+
 ################################################################################
 # Declare ROS messages, services and actions
 ################################################################################
@@ -42,11 +46,12 @@ catkin_package(
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
+  ${yaml_cpp_INCLUDE_DIRS}
 )
 
 add_library(robotis_controller src/robotis_controller/robotis_controller.cpp)
 add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS})
-target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES})
+target_link_libraries(robotis_controller ${yaml_cpp_LIBRARIES} ${catkin_LIBRARIES})
 
 ################################################################################
 # Install
diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml
index dcff20e..72a1c9a 100755
--- a/robotis_controller/package.xml
+++ b/robotis_controller/package.xml
@@ -21,6 +21,7 @@
   <build_depend>robotis_controller_msgs</build_depend>
   <build_depend>robotis_framework_common</build_depend>
   <build_depend>cmake_modules</build_depend>
+  <build_depend>yaml-cpp</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>std_msgs</run_depend>
@@ -30,5 +31,6 @@
   <run_depend>robotis_controller_msgs</run_depend>
   <run_depend>robotis_framework_common</run_depend>
   <run_depend>cmake_modules</run_depend>
+  <run_depend>yaml-cpp</run_depend>
   <export></export>
 </package>