diff --git a/CMakeLists.txt b/CMakeLists.txt
index 364aa95..89167a4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,62 +1,105 @@
-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(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_ros REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(rclcpp REQUIRED)
 
+find_package(Eigen3 REQUIRED)
+find_package(GDAL 3 REQUIRED)
 
-include_directories(
-  include
-)
+find_package(yaml_cpp_vendor REQUIRED)
+link_directories(${yaml_cpp_vendor_LIBRARY_DIRS})
+
+# 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()
 
+# Build
 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})
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen GDAL::GDAL)
+
+ament_target_dependencies(${PROJECT_NAME} SYSTEM
+  grid_map_core
+  grid_map_ros
+)
 
 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})
 
-install(TARGETS
+target_link_libraries(test_tif_loader
+  ${PROJECT_NAME}
+)
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+  ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib/${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION bin/${PROJECT_NAME}
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(GDAL)
+
+# NOTE: if RUNTIME DESTINATION is set to bin/${PROJECT_NAME} the executable
+# is not found by ros2 launch:
+# 
+# Caught exception in launch (see debug for traceback): executable
+# 'test_tif_loader' not found on the libexec directory 
+# '<ws_path>/ros2-aerial/install/grid_map_geo/lib/grid_map_geo'
+install(
+  TARGETS
   test_tif_loader
-  DESTINATION lib/${PROJECT_NAME})
+  ARCHIVE DESTINATION lib/${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
 
 install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME}/
 )
 
+# Test
+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 97%
rename from include/grid_map_geo/grid_map_geo.h
rename to include/grid_map_geo/grid_map_geo.hpp
index 6484ffd..aef380a 100644
--- a/include/grid_map_geo/grid_map_geo.h
+++ b/include/grid_map_geo/grid_map_geo.hpp
@@ -34,16 +34,25 @@
 #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>
 
+#if __APPLE__
+#include <cpl_string.h>
+#include <gdal.h>
+#include <gdal_priv.h>
+#include <ogr_p.h>
+#include <ogr_spatialref.h>
+#else
 #include <gdal/cpl_string.h>
 #include <gdal/gdal.h>
 #include <gdal/gdal_priv.h>
 #include <gdal/ogr_p.h>
 #include <gdal/ogr_spatialref.h>
+#endif
+
 #include <iostream>
 struct Location {
   ESPG espg{ESPG::WGS84};
diff --git a/include/grid_map_geo/transform.h b/include/grid_map_geo/transform.hpp
similarity index 96%
rename from include/grid_map_geo/transform.h
rename to include/grid_map_geo/transform.hpp
index e99531e..d03006e 100644
--- a/include/grid_map_geo/transform.h
+++ b/include/grid_map_geo/transform.hpp
@@ -34,10 +34,17 @@
 #ifndef GRID_MAP_GEO_TRANSFORM_H
 #define GRID_MAP_GEO_TRANSFORM_H
 
+#if __APPLE__
+#include <gdal.h>
+#include <gdal_priv.h>
+#include <ogr_p.h>
+#include <ogr_spatialref.h>
+#else
 #include <gdal/gdal.h>
 #include <gdal/gdal_priv.h>
 #include <gdal/ogr_p.h>
 #include <gdal/ogr_spatialref.h>
+#endif
 
 #include <Eigen/Dense>
 
diff --git a/launch/load_tif.launch.py b/launch/load_tif.launch.py
new file mode 100644
index 0000000..e69628d
--- /dev/null
+++ b/launch/load_tif.launch.py
@@ -0,0 +1,84 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Generate a launch description."""
+
+    pkg_grid_map_geo = get_package_share_directory("grid_map_geo")
+
+    # static transform node
+    static_transform_publisher = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="world_map",
+        arguments=[
+            "--frame-id",
+            "world",
+            "--child-frame-id",
+            "map",
+        ],
+    )
+
+    # tif loader node
+    tif_loader = Node(
+        package="grid_map_geo",
+        namespace="grid_map_geo",
+        executable="test_tif_loader",
+        name="tif_loader",
+        parameters=[
+            {"tif_path": LaunchConfiguration("tif_path")},
+            {"tif_color_path": LaunchConfiguration("tif_color_path")},
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    # rviz node
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=["-d", os.path.join(pkg_grid_map_geo, "launch", "config.rviz")],
+        condition=IfCondition(LaunchConfiguration("rviz")),
+    )
+
+    default_location = "sargans"
+    default_tif_file = "sargans.tif"
+    default_tif_color_file = "sargans_color.tif"
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "rviz", default_value="true", description="Open RViz."
+            ),
+            DeclareLaunchArgument(
+                "location",
+                default_value=default_location,
+                description="Location.",
+            ),
+            DeclareLaunchArgument(
+                "tif_path",
+                default_value=os.path.join(
+                    pkg_grid_map_geo, "resources", default_tif_file
+                ),
+                description="Full path to the elevation map file.",
+            ),
+            DeclareLaunchArgument(
+                "tif_color_path",
+                default_value=os.path.join(
+                    pkg_grid_map_geo, "resources", default_tif_color_file
+                ),
+                description="Full path to the elevation texture file.",
+            ),
+            static_transform_publisher,
+            tif_loader,
+            rviz,
+        ]
+    )
diff --git a/launch/load_tif_launch.xml b/launch/load_tif_launch.xml
new file mode 100644
index 0000000..3f143aa
--- /dev/null
+++ b/launch/load_tif_launch.xml
@@ -0,0 +1,14 @@
+<launch>
+    <arg name="rviz" 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/sargans.tif"/>
+        <param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
+    </node>
+
+    <group if="$(var rviz)">
+        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+    </group>
+</launch>
diff --git a/launch/run.launch.py b/launch/run.launch.py
deleted file mode 100644
index 69c2e6c..0000000
--- a/launch/run.launch.py
+++ /dev/null
@@ -1,30 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-
-def generate_launch_description():
-    package_dir = get_package_share_directory('grid_map_geo')
-    tif_loader = Node(
-        package='grid_map_geo',
-        namespace='grid_map_geo',
-        executable='test_tif_loader',
-        name='tif_loader',
-        output="screen",
-        emulate_tty=True,
-        parameters=[
-                {"tif_path": "/home/jaeyoung/dev/terrain-models/models/hinwil.tif"},
-                {"tif_color_path": "/home/jaeyoung/dev/terrain-models/models/hinwil_color.tif"},
-        ]        
-    )
-    rviz = Node(
-        package='rviz2',
-        namespace='',
-        executable='rviz2',
-        name='rviz2',
-        arguments=['-d', [os.path.join(package_dir, 'launch/config.rviz')]]
-    )
-    return LaunchDescription([
-        tif_loader,
-        rviz
-])
diff --git a/package.xml b/package.xml
index 3d04411..b996158 100644
--- a/package.xml
+++ b/package.xml
@@ -2,18 +2,26 @@
 <?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>
+  <depend>yaml_cpp_vendor</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/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..9132f77 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,11 @@ 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->declare_parameter("tif_path", ".");
+      std::string color_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();
+      RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path);
+      RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
 
       map_ = std::make_shared<GridMapGeo>();
       map_->Load(file_path, false, color_path);
@@ -68,13 +68,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..eaea08e
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,12 @@
+# 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);