diff --git a/CMakeLists.txt b/CMakeLists.txt
index e2e6568..800f710 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_cpp
     robot_time
   )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
 
 # ========================================================
@@ -64,7 +64,6 @@ else()
     robot_laser_geometry
     robot_visualization_msgs
     robot_voxel_grid
-    tf3
     robot_tf3_geometry_msgs
     robot_tf3_sensor_msgs
     data_convert
@@ -73,10 +72,12 @@ else()
     robot_time
   )
 
+  find_library(TF3_LIBRARY NAMES tf3)
+  
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES robot_costmap_2d plugins
-    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
     DEPENDS  PCL Boost 
   )
 
@@ -87,6 +88,7 @@ else()
     ${Boost_INCLUDE_DIRS}
     ${GTEST_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
   )
   link_directories(${PCL_LIBRARY_DIRS})
 endif()
@@ -122,6 +124,7 @@ if(BUILDING_WITH_CATKIN)
       ${EIGEN3_INCLUDE_DIRS}
       ${Boost_INCLUDE_DIRS}
       ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
   )
 
   target_link_libraries(robot_costmap_2d
@@ -130,6 +133,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE yaml-cpp
     PRIVATE dl
     PRIVATE ${PCL_LIBRARIES}
+    PRIVATE ${TF3_LIBRARY}
   )
 
 else()
@@ -141,6 +145,7 @@ else()
       ${EIGEN3_INCLUDE_DIRS}
       ${Boost_INCLUDE_DIRS}
       ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
   )
 
   target_link_libraries(robot_costmap_2d
@@ -151,6 +156,7 @@ else()
       yaml-cpp
       dl
       ${PCL_LIBRARIES}
+      ${TF3_LIBRARY}
   )
 
   set_target_properties(robot_costmap_2d PROPERTIES
@@ -189,6 +195,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE ${catkin_LIBRARIES}
     PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
     PRIVATE yaml-cpp
+    PRIVATE ${TF3_LIBRARY}
   )
 
 else()
@@ -205,6 +212,7 @@ else()
     PRIVATE yaml-cpp
     PRIVATE robot_time
     PRIVATE robot_cpp
+    PRIVATE ${TF3_LIBRARY}
   )
 
   set_target_properties(plugins PROPERTIES
@@ -270,41 +278,47 @@ endif()
 # ========================================================
 # Test executables
 # ========================================================
-# option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" OFF)
+option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
 
-# if(BUILD_COSTMAP_TESTS)
-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
-#     add_executable(test_array_parser test/array_parser_test.cpp)
-#     target_link_libraries(test_array_parser PRIVATE
-#       robot_costmap_2d
-#       GTest::GTest
-#       GTest::Main
-#       Threads::Threads
-#     )
-#   endif()
+if(BUILD_COSTMAP_TESTS)
+  find_package(GTest REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 
-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
-#     add_executable(test_costmap test/coordinates_test.cpp)
-#     target_link_libraries(test_costmap PRIVATE
-#       robot_costmap_2d
-#       GTest::GTest
-#       GTest::Main
-#       Threads::Threads
-#     )
-#   endif()
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
+    add_executable(test_array_parser test/array_parser_test.cpp)
+    target_link_libraries(test_array_parser PRIVATE 
+      robot_costmap_2d
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
+    )
+  endif()
 
-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
-#     add_executable(test_plugin test/static_layer_test.cpp)
-#     target_link_libraries(test_plugin PRIVATE
-#       robot_costmap_2d
-#       Boost::boost Boost::filesystem Boost::system
-#       yaml-cpp
-#       dl
-#       Threads::Threads
-#       tf3
-#       robot_time
-#       GTest::GTest
-#       GTest::Main
-#     )
-#   endif()
-# endif()
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
+    add_executable(test_costmap test/coordinates_test.cpp)
+    target_link_libraries(test_costmap PRIVATE
+      robot_costmap_2d
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
+    )
+  endif()
+
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
+    add_executable(test_plugin test/static_layer_test.cpp)
+    target_link_libraries(test_plugin PRIVATE
+      robot_costmap_2d
+      Boost::boost Boost::filesystem Boost::system
+      yaml-cpp
+      dl
+      Boost::system Boost::thread
+      robot_time
+      GTest::GTest
+      GTest::Main
+      ${TF3_LIBRARY}
+    )
+  endif()
+endif()
diff --git a/package.xml b/package.xml
index 8998e79..68c8e17 100644
--- a/package.xml
+++ b/package.xml
@@ -46,9 +46,6 @@
   <build_depend>robot_voxel_grid</build_depend>
   <run_depend>robot_voxel_grid</run_depend>
 
-  <build_depend>tf3</build_depend>
-  <run_depend>tf3</run_depend>
-
   <build_depend>robot_tf3_geometry_msgs</build_depend>
   <run_depend>robot_tf3_geometry_msgs</run_depend>