diff --git a/eagleye_core/coordinate/CMakeLists.txt b/eagleye_core/coordinate/CMakeLists.txt
index eaa7d484..00e15219 100755
--- a/eagleye_core/coordinate/CMakeLists.txt
+++ b/eagleye_core/coordinate/CMakeLists.txt
@@ -15,12 +15,12 @@ ament_auto_find_build_dependencies()
 
 find_package(PkgConfig)
 find_package(geodesy)
+
+# GeographicLib
 find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
-  PAHT_SUFFIXES GeographicLib
-)
-set(GeographicLib_LIBRARIES
-  NAMES Geographic
+  PATH_SUFFIXES GeographicLib
 )
+find_library(GeographicLib_LIBRARIES NAMES Geographic GeographicLib)
 
 include_directories(
   include
@@ -50,7 +50,7 @@ ament_export_include_directories(include)
 ament_target_dependencies(eagleye_coordinate)
 
 target_link_libraries(eagleye_coordinate
-  Geographic
+  ${GeographicLib_LIBRARIES}
 )
 
 install(
diff --git a/eagleye_core/coordinate/package.xml b/eagleye_core/coordinate/package.xml
index fe132011..3b3b69e5 100644
--- a/eagleye_core/coordinate/package.xml
+++ b/eagleye_core/coordinate/package.xml
@@ -13,6 +13,7 @@
   <build_depend>geodesy</build_depend>
   <build_export_depend>rclcpp</build_export_depend>
   <build_export_depend>geodesy</build_export_depend>
+  <depend>ament_index_cpp</depend>
   <depend>eigen</depend>
   <depend>geographic_info</depend>
   <depend>geographic_msgs</depend>
diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt
index 5a45e60c..a5737a9d 100755
--- a/eagleye_core/navigation/CMakeLists.txt
+++ b/eagleye_core/navigation/CMakeLists.txt
@@ -19,6 +19,7 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 find_package(eagleye_coordinate REQUIRED)
 find_package(PkgConfig REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 ament_auto_find_build_dependencies()
 
@@ -47,7 +48,7 @@ ament_auto_add_library(eagleye_navigation SHARED
   include/eagleye_navigation/eagleye_navigation.hpp
 )
 
-ament_auto_find_build_dependencies()
+target_link_libraries(eagleye_navigation yaml-cpp)
 
 install(TARGETS eagleye_navigation
   INCLUDES DESTINATION include
diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt
index 37a85a04..8ac9becb 100644
--- a/eagleye_rt/CMakeLists.txt
+++ b/eagleye_rt/CMakeLists.txt
@@ -37,14 +37,23 @@ ament_auto_add_executable(distance
 ament_auto_add_executable(heading_interpolate
   src/heading_interpolate_node.cpp
 )
+target_link_libraries(heading_interpolate
+  yaml-cpp
+)
 
 ament_auto_add_executable(heading
   src/heading_node.cpp
 )
+target_link_libraries(heading
+  yaml-cpp
+)
 
 ament_auto_add_executable(height
   src/height_node.cpp
 )
+target_link_libraries(height
+  yaml-cpp
+)
 
 ament_auto_add_executable(monitor
   src/monitor_node.cpp
@@ -53,62 +62,107 @@ ament_auto_add_executable(monitor
 ament_auto_add_executable(position_interpolate
   src/position_interpolate_node.cpp
 )
+target_link_libraries(position_interpolate
+  yaml-cpp
+)
 
 ament_auto_add_executable(position
   src/position_node.cpp
 )
+target_link_libraries(position
+  yaml-cpp
+)
 
 ament_auto_add_executable(slip_angle
   src/slip_angle_node.cpp
 )
+target_link_libraries(slip_angle
+  yaml-cpp
+)
 
 ament_auto_add_executable(slip_coefficient
   src/slip_coefficient_node.cpp
 )
+target_link_libraries(slip_coefficient
+  yaml-cpp
+)
 
 ament_auto_add_executable(enable_additional_rolling
   src/enable_additional_rolling_node.cpp
 )
+target_link_libraries(enable_additional_rolling
+  yaml-cpp
+)
 
 ament_auto_add_executable(rolling
   src/rolling_node.cpp
 )
+target_link_libraries(rolling
+  yaml-cpp
+)
 
 ament_auto_add_executable(smoothing
   src/smoothing_node.cpp
 )
+target_link_libraries(smoothing
+  yaml-cpp
+)
 
 ament_auto_add_executable(trajectory
   src/trajectory_node.cpp
 )
+target_link_libraries(trajectory
+  yaml-cpp
+)
 
 ament_auto_add_executable(velocity_scale_factor
   src/velocity_scale_factor_node.cpp
 )
+target_link_libraries(velocity_scale_factor
+  yaml-cpp
+)
 
 ament_auto_add_executable(yaw_rate_offset
   src/yaw_rate_offset_node.cpp
 )
+target_link_libraries(yaw_rate_offset
+  yaml-cpp
+)
 
 ament_auto_add_executable(yaw_rate_offset_stop
   src/yaw_rate_offset_stop_node.cpp
 )
+target_link_libraries(yaw_rate_offset_stop
+  yaml-cpp
+)
 
 ament_auto_add_executable(angular_velocity_offset_stop
   src/angular_velocity_offset_stop_node.cpp
 )
+target_link_libraries(angular_velocity_offset_stop
+  yaml-cpp
+)
 
 ament_auto_add_executable(rtk_dead_reckoning
   src/rtk_dead_reckoning_node.cpp
 )
+target_link_libraries(rtk_dead_reckoning
+  yaml-cpp
+)
 
 ament_auto_add_executable(rtk_heading
   src/rtk_heading_node.cpp
 )
+target_link_libraries(rtk_heading
+  yaml-cpp
+)
 
 ament_auto_add_executable(velocity_estimator
   src/velocity_estimator_node.cpp
 )
+target_link_libraries(velocity_estimator
+  yaml-cpp
+)
 
 install(TARGETS 
   tf_converted_imu
diff --git a/eagleye_rt/src/tf_converted_imu.cpp b/eagleye_rt/src/tf_converted_imu.cpp
index 3c9c64a8..51f8fb68 100644
--- a/eagleye_rt/src/tf_converted_imu.cpp
+++ b/eagleye_rt/src/tf_converted_imu.cpp
@@ -36,14 +36,11 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
+
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #include <tf2/transform_datatypes.h>
-#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 class TFConvertedIMU: public rclcpp::Node
 {
diff --git a/eagleye_util/geo_pose_fusion/package.xml b/eagleye_util/geo_pose_fusion/package.xml
index 66c0170b..90f4aff6 100644
--- a/eagleye_util/geo_pose_fusion/package.xml
+++ b/eagleye_util/geo_pose_fusion/package.xml
@@ -23,6 +23,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>tf2_geometry_msgs</depend>
 
   <export>