diff --git a/CMakeLists.txt b/CMakeLists.txt
index 364aa95..39c5af0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,62 +1,68 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.14.4)
 project(grid_map_geo)
 
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+# Set policy for 3.16 for CMP0076 defaulting to ON
+cmake_policy(VERSION 3.16)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
-find_package(GDAL REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3)
-
+find_package(GDAL 3 REQUIRED)
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(grid_map_core REQUIRED)
 find_package(grid_map_ros REQUIRED)
-find_package(grid_map_msgs REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(Eigen3 REQUIRED)
 
+add_library(${PROJECT_NAME})
+add_subdirectory(src)
 
-include_directories(
-  include
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
 
-add_library(${PROJECT_NAME}
-  src/grid_map_geo.cpp
-)
-ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs geometry_msgs grid_map_core grid_map_ros grid_map_msgs)
-target_include_directories(${PROJECT_NAME} PUBLIC "${Eigen3_INCLUDE_DIRS}")
-target_link_libraries(${PROJECT_NAME} ${GDAL_LIBRARY})
+# Reverse compatability for GDAL<3.5
+# https://gdal.org/development/cmake.html
+if(NOT TARGET GDAL::GDAL)
+    add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY})
+endif()
 
-add_executable(test_tif_loader
-  src/test_tif_loader.cpp
-)
-ament_target_dependencies(test_tif_loader rclcpp std_msgs)
-target_include_directories(test_tif_loader PUBLIC "${Eigen3_INCLUDE_DIRS}")
-target_link_libraries(test_tif_loader ${PROJECT_NAME} ${GDAL_LIBRARY})
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL grid_map_core::grid_map_core)
+
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(GDAL)
 
-install(TARGETS
-  test_tif_loader
-  DESTINATION lib/${PROJECT_NAME})
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
 install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME}/
 )
 
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}Targets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+include(CTest)
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+    add_subdirectory(test)
 endif()
 
 ament_package()
diff --git a/README.md b/README.md
index 8a5ac56..c265942 100644
--- a/README.md
+++ b/README.md
@@ -10,30 +10,22 @@ This package provides a georeferenced extension to the elevation map [grid_map](
 Affiliation: [ETH Zurich, Autonomous Systems Lab](https://asl.ethz.ch/)<br />**
 ## Setup
 Install the dependencies. This package depends on gdal, to read georeferenced images and GeoTIFF files.
-```
-apt install libgdal-dev
-```
-Configure the catkin workspace
-```
-catkin config --extend "/opt/ros/noetic"
-catkin config --merge-devel
-```
 
-Pull in dependencies using rosinstall / rosdep
+Pull in dependencies using rosdep
 ```
-wstool init src src/grid_map_geo/dependencies.rosinstall
-wstool update -t src -j4
+source /opt/ros/humble/setup.bash
 rosdep update
-rosdep install --from-paths src --ignore-src -y --rosdistro noetic
+# Assuming the package is cloned in the src folder of a ROS workspace...
+rosdep install --from-paths src --ignore-src -y
 ```
 
 Build the package
 ```
-catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False
-catkin build -j$(nproc) -l$(nproc) grid_map_geo
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False --packages-up-to grid_map_geo
 ```
 ## Running the package
 The default launch file can be run as the following command. 
 ```
-roslaunch grid_map_geo load_tif.launch
+source install/setup.bash
+ros2 launch grid_map_geo load_tif_launch.xml
 ```
diff --git a/include/grid_map_geo/grid_map_geo.h b/include/grid_map_geo/grid_map_geo.hpp
similarity index 99%
rename from include/grid_map_geo/grid_map_geo.h
rename to include/grid_map_geo/grid_map_geo.hpp
index 6484ffd..c2b962c 100644
--- a/include/grid_map_geo/grid_map_geo.h
+++ b/include/grid_map_geo/grid_map_geo.hpp
@@ -34,7 +34,7 @@
 #ifndef GRID_MAP_GEO_H
 #define GRID_MAP_GEO_H
 
-#include "grid_map_geo/transform.h"
+#include "grid_map_geo/transform.hpp"
 
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
diff --git a/include/grid_map_geo/transform.h b/include/grid_map_geo/transform.hpp
similarity index 100%
rename from include/grid_map_geo/transform.h
rename to include/grid_map_geo/transform.hpp
diff --git a/launch/load_tif_launch.xml b/launch/load_tif_launch.xml
new file mode 100644
index 0000000..749a34d
--- /dev/null
+++ b/launch/load_tif_launch.xml
@@ -0,0 +1,14 @@
+<launch>
+    <arg name="visualization" default="true"/>
+    <arg name="location" default="sargans"/>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
+
+    <node pkg="grid_map_geo" exec="test_tif_loader" name="test_tif_loader" output="screen">
+        <param name="tif_path" value="$(find-pkg-share grid_map_geo)/resources/sertig.tif"/>
+        <param name="color_path" value="$(find-pkg-share grid_map_geo)/resources/sertig_color.tif"/>
+    </node>
+
+    <group if="$(var visualization)">
+        <node exec="rviz" name="rviz" pkg="rviz"  args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+    </group>
+</launch>
diff --git a/package.xml b/package.xml
index 3d04411..823838d 100644
--- a/package.xml
+++ b/package.xml
@@ -2,18 +2,25 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>grid_map_geo</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="jalim@ethz.ch">jaeyoung</maintainer>
-  <license>TODO: License declaration</license>
+  <version>0.0.1</version>
+  <description>Georeferenced grid map</description>
+  <maintainer email="jalim@ethz.ch">Jaeyoung Lim</maintainer>
+  <maintainer email="ryanfriedman5410+github@gmail.com">Ryan Friedman</maintainer>
+  <author>Jaeyoung Lim</author>
+  <license>BSD-3</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <url type="website">https://github.com/Jaeyoung-Lim/grid_map_geo</url>
+  <url type="bugtracker">https://github.com/Jaeyoung-Lim/grid_map_geo/issues</url>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <depend>rclcpp</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>eigen</depend>
   <depend>grid_map_core</depend>
+  <depend>grid_map_ros</depend>
+  <depend>libgdal-dev</depend>
+  <depend>rclcpp</depend>
+  <exec_depend>ros2launch</exec_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
\ No newline at end of file
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..9352845
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,10 @@
+target_sources(${PROJECT_NAME} PRIVATE grid_map_geo.cpp)
+
+
+find_package(rclcpp REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(grid_map_ros REQUIRED)
+add_executable(test_tif_loader)
+target_sources(test_tif_loader PRIVATE test_tif_loader.cpp)
+target_link_libraries(test_tif_loader PRIVATE ${PROJECT_NAME} rclcpp::rclcpp ${grid_map_msgs_TARGETS})
+ament_target_dependencies(${PROJECT_NAME} grid_map_ros)
\ No newline at end of file
diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
index 8b01ec2..355908e 100644
--- a/src/grid_map_geo.cpp
+++ b/src/grid_map_geo.cpp
@@ -37,7 +37,7 @@
  * @author Jaeyoung Lim <jalim@ethz.ch>
  */
 
-#include "grid_map_geo/grid_map_geo.h"
+#include "grid_map_geo/grid_map_geo.hpp"
 
 #include <grid_map_core/GridMapMath.hpp>
 #include <grid_map_core/iterators/CircleIterator.hpp>
diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp
index c42f3b8..1c8636d 100644
--- a/src/test_tif_loader.cpp
+++ b/src/test_tif_loader.cpp
@@ -37,7 +37,7 @@
  * @author Jaeyoung Lim <jalim@ethz.ch>
  */
 
-#include "grid_map_geo/grid_map_geo.h"
+#include "grid_map_geo/grid_map_geo.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -52,11 +52,8 @@ class MapPublisher : public rclcpp::Node {
     MapPublisher() : Node("map_publisher") {
       original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", 1);
 
-      this->declare_parameter("tif_path", ".");
-      this->declare_parameter("tif_color_path", ".");
-
-      std::string file_path = this->get_parameter("tif_path").as_string();
-      std::string color_path = this->get_parameter("tif_color_path").as_string();
+      std::string file_path = this->declare_parameter("tif_path", ".");
+      std::string color_path = this->declare_parameter("tif_color_path", ".");
 
       map_ = std::make_shared<GridMapGeo>();
       map_->Load(file_path, false, color_path);
@@ -68,13 +65,8 @@ class MapPublisher : public rclcpp::Node {
       original_map_pub_->publish(std::move(msg));
     }
     rclcpp::TimerBase::SharedPtr timer_;
-    size_t count_{0};
     rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;
     std::shared_ptr<GridMapGeo> map_;
-  // void MapPublishOnce(ros::Publisher &pub, grid_map::GridMap &map) {
-  //   map.setTimestamp(ros::Time::now().toNSec());
-  //   pub.publish(message);
-  // }
 };
 
 int main(int argc, char **argv) {
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000..08aad80
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,13 @@
+# Add gtest based cpp test target and link libraries
+find_package(ament_cmake_gtest)
+ament_add_gtest(${PROJECT_NAME}-test 
+    main.cpp
+    test_grid_map_geo.cpp
+)
+
+
+target_link_libraries(${PROJECT_NAME}-test
+    ${PROJECT_NAME}
+        # ${YAML_CPP_LIBRARIES}
+        # ${GDAL_LIBRARY}
+)
diff --git a/test/test_grid_map_geo.cpp b/test/test_grid_map_geo.cpp
index 05bad62..cb1c005 100644
--- a/test/test_grid_map_geo.cpp
+++ b/test/test_grid_map_geo.cpp
@@ -1,15 +1,10 @@
-#include "grid_map_geo/transform.h"
+#include "grid_map_geo/transform.hpp"
 
 #include <gtest/gtest.h>
 #include <iostream>
 
 TEST(GridMapTest, geoTransform) {
-  // Depending on Gdal versions, lon lat order are reversed
-#if GDAL_VERSION_MAJOR > 2
   Eigen::Vector3d berghaus_wgs84(46.7209147, 9.9219592, 488.0);
-#else
-  Eigen::Vector3d berghaus_wgs84(9.9219592, 46.7209147, 488.0);
-#endif
   Eigen::Vector3d berghaus_lv03(789823.96735451114, 177416.47911055354, 440.3752994351089);
   Eigen::Vector3d tranformed_lv03 = transformCoordinates(ESPG::WGS84, ESPG::CH1903_LV03, berghaus_wgs84);
   EXPECT_NEAR(tranformed_lv03(0), berghaus_lv03(0), 0.0001);