diff --git a/.gitignore b/.gitignore
index 7411f329..1cc7753a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@ __pycache__
 .vscode/
 
 watod-config.local.sh
+build
\ No newline at end of file
diff --git a/README.md b/README.md
index b293c8dd..c7a904a2 100644
--- a/README.md
+++ b/README.md
@@ -1,12 +1,28 @@
-# WATonomous ASD Admissions Assignment
+## Project Demo
+This video demonstrates the current prototype of our autonomous robot, showcasing LiDAR-based SLAM mapping and path planning in action.  
+
+[![Watch the Demo](https://img.youtube.com/vi/wIKEoOrXFpg/0.jpg)](https://www.youtube.com/watch?v=wIKEoOrXFpg)  
+*Click the image to watch the full demo on YouTube.*
 
 ## Prerequisite Installation
-These steps are to setup the monorepo to work on your own PC. We utilize docker to enable ease of reproducibility and deployability.
 
-> Why docker? It's so that you don't need to download any coding libraries on your bare metal pc, saving headache :3
+These steps will set up the monorepo on your local machine. We use **Docker** to ensure reproducibility and simplify deployment.  
+
+1. **Operating System Support:**  
+   This project supports **Linux (Ubuntu ≥ 22.04), Windows (via WSL), and macOS**. You can set up your environment using one of the following approaches:  
+   - [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview)  
+   - [Windows Subsystem for Linux (WSL)](https://learn.microsoft.com/en-us/windows/wsl/install)  
+   - [Dual Boot Linux Setup](https://opensource.com/article/18/5/dual-boot-linux)  
 
-1. This assignment is supported on Linux Ubuntu >= 22.04, Windows (WSL), and MacOS. This is standard practice that roboticists can't get around. To setup, you can either setup an [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview), setting up [WSL](https://learn.microsoft.com/en-us/windows/wsl/install), or setting up your computer to [dual boot](https://opensource.com/article/18/5/dual-boot-linux). You can find online resources for all three approaches.
-2. Once inside Linux, [Download Docker Engine using the `apt` repository](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository)
-3. You're all set! You can begin the assignment by visiting the WATonomous Wiki.
+2. **Install Docker:**  
+   Once your Linux environment is ready, follow the [official Docker installation guide](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository) to install Docker Engine.  
 
-Link to Onboarding Assignment: https://wiki.watonomous.ca/
+3. **Build and Run the Project:**  
+   - Build the project:  
+     ```bash
+     ./watod build
+     ```  
+   - Start the simulation locally on Foxglobe:  
+     ```bash
+     ./watod up
+     ```  
diff --git a/frames_2026-01-09_00.35.06.gv b/frames_2026-01-09_00.35.06.gv
new file mode 100644
index 00000000..dc986e8e
--- /dev/null
+++ b/frames_2026-01-09_00.35.06.gv
@@ -0,0 +1,14 @@
+digraph G {
+"robot" -> "robot/right_wheel"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"sim_world" -> "robot"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot" -> "robot/left_wheel"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"robot" -> "robot/caster"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"robot/chassis" -> "robot/chassis/camera"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot" -> "robot/chassis"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot/chassis" -> "robot/chassis/lidar"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot/chassis" -> "robot/chassis/imu_sensor"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1767936906.2349072"[ shape=plaintext ] ;
+}->"sim_world";
+}
\ No newline at end of file
diff --git a/frames_2026-01-09_00.35.06.pdf b/frames_2026-01-09_00.35.06.pdf
new file mode 100644
index 00000000..bfe59449
Binary files /dev/null and b/frames_2026-01-09_00.35.06.pdf differ
diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout
new file mode 100644
index 00000000..3aad5336
--- /dev/null
+++ b/install/.colcon_install_layout
@@ -0,0 +1 @@
+isolated
diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/log/build_2026-01-08_08-26-27/logger_all.log b/log/build_2026-01-08_08-26-27/logger_all.log
new file mode 100644
index 00000000..d5c82129
--- /dev/null
+++ b/log/build_2026-01-08_08-26-27/logger_all.log
@@ -0,0 +1,400 @@
+[0.365s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build']
+[0.365s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=8, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7da5c187ab40>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7da5c187a7e0>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7da5c187a7e0>>, mixin_verb=('build',))
+[0.431s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
+[0.432s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
+[0.432s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
+[0.432s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
+[0.432s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
+[0.432s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
+[0.432s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/ge3626/wato_asd_training'
+[0.432s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
+[0.432s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
+[0.432s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
+[0.433s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
+[0.475s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['ignore', 'ignore_ament_install']
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'ignore'
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'ignore_ament_install'
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['colcon_pkg']
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'colcon_pkg'
+[0.476s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['colcon_meta']
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'colcon_meta'
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['ros']
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'ros'
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['cmake', 'python']
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'cmake'
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'python'
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extensions ['python_setup_py']
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(config) by extension 'python_setup_py'
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['ignore', 'ignore_ament_install']
+[0.477s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'ignore'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'ignore_ament_install'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['colcon_pkg']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'colcon_pkg'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['colcon_meta']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'colcon_meta'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['ros']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'ros'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['cmake', 'python']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'cmake'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'python'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extensions ['python_setup_py']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker) by extension 'python_setup_py'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['ignore', 'ignore_ament_install']
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'ignore'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'ignore_ament_install'
+[0.478s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['colcon_pkg']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'colcon_pkg'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['colcon_meta']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'colcon_meta'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['ros']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'ros'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['cmake', 'python']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'cmake'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'python'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extensions ['python_setup_py']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/gazebo) by extension 'python_setup_py'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['ignore', 'ignore_ament_install']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'ignore'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'ignore_ament_install'
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['colcon_pkg']
+[0.479s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'colcon_pkg'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['colcon_meta']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'colcon_meta'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['ros']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'ros'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['cmake', 'python']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'cmake'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'python'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extensions ['python_setup_py']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/robot) by extension 'python_setup_py'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['ignore', 'ignore_ament_install']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'ignore'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'ignore_ament_install'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['colcon_pkg']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'colcon_pkg'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['colcon_meta']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'colcon_meta'
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['ros']
+[0.480s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'ros'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['cmake', 'python']
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'cmake'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'python'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extensions ['python_setup_py']
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/samples) by extension 'python_setup_py'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['ignore', 'ignore_ament_install']
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'ignore'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'ignore_ament_install'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['colcon_pkg']
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'colcon_pkg'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['colcon_meta']
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'colcon_meta'
+[0.481s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['ros']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'ros'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['cmake', 'python']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'cmake'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'python'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extensions ['python_setup_py']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(docker/vis_tools) by extension 'python_setup_py'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['ignore', 'ignore_ament_install']
+[0.482s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'ignore'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'ignore_ament_install'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['colcon_pkg']
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'colcon_pkg'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['colcon_meta']
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'colcon_meta'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['ros']
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'ros'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['cmake', 'python']
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'cmake'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'python'
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extensions ['python_setup_py']
+[0.483s] Level 1:colcon.colcon_core.package_identification:_identify(modules) by extension 'python_setup_py'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extensions ['ignore', 'ignore_ament_install']
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extension 'ignore'
+[0.484s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extension 'ignore_ament_install'
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extensions ['colcon_pkg']
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extension 'colcon_pkg'
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extensions ['colcon_meta']
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extension 'colcon_meta'
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extensions ['ros']
+[0.485s] Level 1:colcon.colcon_core.package_identification:_identify(src/gazebo) by extension 'ros'
+[0.488s] DEBUG:colcon.colcon_core.package_identification:Package 'src/gazebo' with type 'ros.ament_cmake' and name 'gazebo'
+[0.488s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['ignore', 'ignore_ament_install']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'ignore'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'ignore_ament_install'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['colcon_pkg']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'colcon_pkg'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['colcon_meta']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'colcon_meta'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['ros']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'ros'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['cmake', 'python']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'cmake'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'python'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extensions ['python_setup_py']
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot) by extension 'python_setup_py'
+[0.489s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extensions ['ignore', 'ignore_ament_install']
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extension 'ignore'
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extension 'ignore_ament_install'
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extensions ['colcon_pkg']
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extension 'colcon_pkg'
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extensions ['colcon_meta']
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extension 'colcon_meta'
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extensions ['ros']
+[0.490s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/bringup_robot) by extension 'ros'
+[0.491s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/bringup_robot' with type 'ros.ament_cmake' and name 'bringup_robot'
+[0.492s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extensions ['ignore', 'ignore_ament_install']
+[0.492s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extension 'ignore'
+[0.492s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extension 'ignore_ament_install'
+[0.500s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extensions ['colcon_pkg']
+[0.500s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extension 'colcon_pkg'
+[0.500s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extensions ['colcon_meta']
+[0.500s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extension 'colcon_meta'
+[0.500s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extensions ['ros']
+[0.501s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/control) by extension 'ros'
+[0.502s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/control' with type 'ros.ament_cmake' and name 'control'
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extensions ['ignore', 'ignore_ament_install']
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extension 'ignore'
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extension 'ignore_ament_install'
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extensions ['colcon_pkg']
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extension 'colcon_pkg'
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extensions ['colcon_meta']
+[0.502s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extension 'colcon_meta'
+[0.503s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extensions ['ros']
+[0.503s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/costmap) by extension 'ros'
+[0.504s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/costmap' with type 'ros.ament_cmake' and name 'costmap'
+[0.504s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extensions ['ignore', 'ignore_ament_install']
+[0.504s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extension 'ignore'
+[0.504s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extension 'ignore_ament_install'
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extensions ['colcon_pkg']
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extension 'colcon_pkg'
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extensions ['colcon_meta']
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extension 'colcon_meta'
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extensions ['ros']
+[0.505s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/map_memory) by extension 'ros'
+[0.506s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/map_memory' with type 'ros.ament_cmake' and name 'map_memory'
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extensions ['ignore', 'ignore_ament_install']
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extension 'ignore'
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extension 'ignore_ament_install'
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extensions ['colcon_pkg']
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extension 'colcon_pkg'
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extensions ['colcon_meta']
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extension 'colcon_meta'
+[0.506s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extensions ['ros']
+[0.507s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/odometry_spoof) by extension 'ros'
+[0.507s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/odometry_spoof' with type 'ros.ament_cmake' and name 'odometry_spoof'
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extensions ['ignore', 'ignore_ament_install']
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extension 'ignore'
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extension 'ignore_ament_install'
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extensions ['colcon_pkg']
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extension 'colcon_pkg'
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extensions ['colcon_meta']
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extension 'colcon_meta'
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extensions ['ros']
+[0.508s] Level 1:colcon.colcon_core.package_identification:_identify(src/robot/planner) by extension 'ros'
+[0.509s] DEBUG:colcon.colcon_core.package_identification:Package 'src/robot/planner' with type 'ros.ament_cmake' and name 'planner'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['ignore', 'ignore_ament_install']
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'ignore'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'ignore_ament_install'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['colcon_pkg']
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'colcon_pkg'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['colcon_meta']
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'colcon_meta'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['ros']
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'ros'
+[0.509s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['cmake', 'python']
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'cmake'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'python'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extensions ['python_setup_py']
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples) by extension 'python_setup_py'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['ignore', 'ignore_ament_install']
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'ignore'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'ignore_ament_install'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['colcon_pkg']
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'colcon_pkg'
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['colcon_meta']
+[0.510s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'colcon_meta'
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['ros']
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'ros'
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['cmake', 'python']
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'cmake'
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'python'
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extensions ['python_setup_py']
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp) by extension 'python_setup_py'
+[0.511s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extensions ['ignore', 'ignore_ament_install']
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extension 'ignore'
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extension 'ignore_ament_install'
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extensions ['colcon_pkg']
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extension 'colcon_pkg'
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extensions ['colcon_meta']
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extension 'colcon_meta'
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extensions ['ros']
+[0.512s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/aggregator) by extension 'ros'
+[0.513s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/cpp/aggregator' with type 'ros.ament_cmake' and name 'aggregator'
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extensions ['ignore', 'ignore_ament_install']
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extension 'ignore'
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extension 'ignore_ament_install'
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extensions ['colcon_pkg']
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extension 'colcon_pkg'
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extensions ['colcon_meta']
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extension 'colcon_meta'
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extensions ['ros']
+[0.513s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/producer) by extension 'ros'
+[0.514s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/cpp/producer' with type 'ros.ament_cmake' and name 'producer'
+[0.514s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extensions ['ignore', 'ignore_ament_install']
+[0.514s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extension 'ignore'
+[0.514s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extension 'ignore_ament_install'
+[0.514s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extensions ['colcon_pkg']
+[0.514s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extension 'colcon_pkg'
+[0.515s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extensions ['colcon_meta']
+[0.515s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extension 'colcon_meta'
+[0.515s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extensions ['ros']
+[0.515s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/cpp/transformer) by extension 'ros'
+[0.515s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/cpp/transformer' with type 'ros.ament_cmake' and name 'transformer'
+[0.515s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['ignore', 'ignore_ament_install']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'ignore'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'ignore_ament_install'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['colcon_pkg']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'colcon_pkg'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['colcon_meta']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'colcon_meta'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['ros']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'ros'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['cmake', 'python']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'cmake'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'python'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extensions ['python_setup_py']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python) by extension 'python_setup_py'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extensions ['ignore', 'ignore_ament_install']
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extension 'ignore'
+[0.516s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extension 'ignore_ament_install'
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extensions ['colcon_pkg']
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extension 'colcon_pkg'
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extensions ['colcon_meta']
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extension 'colcon_meta'
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extensions ['ros']
+[0.517s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/aggregator) by extension 'ros'
+[0.518s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/python/aggregator' with type 'ros.ament_python' and name 'aggregator'
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extensions ['ignore', 'ignore_ament_install']
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extension 'ignore'
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extension 'ignore_ament_install'
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extensions ['colcon_pkg']
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extension 'colcon_pkg'
+[0.518s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extensions ['colcon_meta']
+[0.519s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extension 'colcon_meta'
+[0.519s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extensions ['ros']
+[0.519s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/producer) by extension 'ros'
+[0.520s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/python/producer' with type 'ros.ament_python' and name 'producer'
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extensions ['ignore', 'ignore_ament_install']
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extension 'ignore'
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extension 'ignore_ament_install'
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extensions ['colcon_pkg']
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extension 'colcon_pkg'
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extensions ['colcon_meta']
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extension 'colcon_meta'
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extensions ['ros']
+[0.520s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/python/transformer) by extension 'ros'
+[0.521s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/python/transformer' with type 'ros.ament_python' and name 'transformer'
+[0.521s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extensions ['ignore', 'ignore_ament_install']
+[0.521s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extension 'ignore'
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extension 'ignore_ament_install'
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extensions ['colcon_pkg']
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extension 'colcon_pkg'
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extensions ['colcon_meta']
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extension 'colcon_meta'
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extensions ['ros']
+[0.522s] Level 1:colcon.colcon_core.package_identification:_identify(src/samples/sample_msgs) by extension 'ros'
+[0.523s] DEBUG:colcon.colcon_core.package_identification:Package 'src/samples/sample_msgs' with type 'ros.ament_cmake' and name 'sample_msgs'
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['ignore', 'ignore_ament_install']
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'ignore'
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'ignore_ament_install'
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['colcon_pkg']
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'colcon_pkg'
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['colcon_meta']
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'colcon_meta'
+[0.523s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['ros']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'ros'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['cmake', 'python']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'cmake'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'python'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extensions ['python_setup_py']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs) by extension 'python_setup_py'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extensions ['ignore', 'ignore_ament_install']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extension 'ignore'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extension 'ignore_ament_install'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extensions ['colcon_pkg']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extension 'colcon_pkg'
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extensions ['colcon_meta']
+[0.524s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extension 'colcon_meta'
+[0.525s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extensions ['ros']
+[0.525s] Level 1:colcon.colcon_core.package_identification:_identify(src/wato_msgs/sample_msgs) by extension 'ros'
+[0.525s] DEBUG:colcon.colcon_core.package_identification:Package 'src/wato_msgs/sample_msgs' with type 'ros.ament_cmake' and name 'sample_msgs'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['ignore', 'ignore_ament_install']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'ignore'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'ignore_ament_install'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['colcon_pkg']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'colcon_pkg'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['colcon_meta']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'colcon_meta'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['ros']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'ros'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['cmake', 'python']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'cmake'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'python'
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extensions ['python_setup_py']
+[0.526s] Level 1:colcon.colcon_core.package_identification:_identify(watod_scripts) by extension 'python_setup_py'
+[0.527s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
+[0.527s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
+[0.527s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
+[0.527s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
+[0.527s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
+[0.571s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters
+[0.571s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover
+[0.578s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 399 installed packages in /opt/ros/kilted
+[0.579s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults
+[0.659s] ERROR:colcon:colcon build: Duplicate package names not supported:
+- aggregator:
+  - src/samples/cpp/aggregator
+  - src/samples/python/aggregator
+- producer:
+  - src/samples/cpp/producer
+  - src/samples/python/producer
+- sample_msgs:
+  - src/samples/sample_msgs
+  - src/wato_msgs/sample_msgs
+- transformer:
+  - src/samples/cpp/transformer
+  - src/samples/python/transformer
diff --git a/log/latest b/log/latest
new file mode 120000
index 00000000..b57d247c
--- /dev/null
+++ b/log/latest
@@ -0,0 +1 @@
+latest_build
\ No newline at end of file
diff --git a/log/latest_build b/log/latest_build
new file mode 120000
index 00000000..69a4a5c4
--- /dev/null
+++ b/log/latest_build
@@ -0,0 +1 @@
+build_2026-01-08_08-26-27
\ No newline at end of file
diff --git a/src/robot/control/CMakeLists.txt b/src/robot/control/CMakeLists.txt
index 01191099..24d2e1c8 100644
--- a/src/robot/control/CMakeLists.txt
+++ b/src/robot/control/CMakeLists.txt
@@ -13,6 +13,8 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -27,8 +29,10 @@ target_include_directories(control_lib
   include)
 
 # Add ROS2 dependencies required by package
-ament_target_dependencies(control_lib
-  rclcpp
+target_link_libraries(control_lib
+  rclcpp::rclcpp
+  ${nav_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
 )
 
 # Create ROS2 node executable from source files
diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp
index d47f95b5..0287c15f 100644
--- a/src/robot/control/include/control_node.hpp
+++ b/src/robot/control/include/control_node.hpp
@@ -2,6 +2,10 @@
 #define CONTROL_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
 
 #include "control_core.hpp"
 
@@ -11,6 +15,23 @@ class ControlNode : public rclcpp::Node {
 
   private:
     robot::ControlCore control_;
+    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    
+    nav_msgs::msg::Path path;
+    nav_msgs::msg::Odometry odom;
+    geometry_msgs::msg::PoseStamped goal;
+    geometry_msgs::msg::PoseStamped target;
+
+    int64_t dt = 50; //ms
+    const double max_linear_speed = 2.0;
+    const double max_angular_speed = 2.0;
+    const double lookahead_distance = 1.5;
+    
+    void periodic();
+    bool is_near(double target_x, double target_y, double tolerance);
 };
 
 #endif
diff --git a/src/robot/control/package.xml b/src/robot/control/package.xml
index b9b7cd6c..bcf067be 100644
--- a/src/robot/control/package.xml
+++ b/src/robot/control/package.xml
@@ -10,6 +10,8 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp
index c99e2004..1e610bb1 100644
--- a/src/robot/control/src/control_node.cpp
+++ b/src/robot/control/src/control_node.cpp
@@ -1,6 +1,82 @@
 #include "control_node.hpp"
 
-ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {}
+ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {
+  path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+    "/path", 10, 
+    [this](const nav_msgs::msg::Path::SharedPtr msg) { 
+      path = *msg; 
+      goal = path.poses.back();
+  });
+  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "/odom/filtered", 10, 
+    [this](const nav_msgs::msg::Odometry::SharedPtr msg) { odom = *msg; });
+  cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(dt), [this]() { periodic(); });
+}
+
+void ControlNode::periodic() {
+  if (path.poses.empty()) {
+    return;
+  }
+
+  target = path.poses.front();
+
+  const size_t n = path.poses.size();
+  double distance = 0.0;
+  for (size_t i = 0; i + 1 < n; i++) {
+    double x1 = path.poses[i].pose.position.x;
+    double y1 = path.poses[i].pose.position.y;
+    double x2 = path.poses[i + 1].pose.position.x;
+    double y2 = path.poses[i + 1].pose.position.y;
+    distance += std::hypot(x1 - x2, y1 - y2);
+
+    if (distance >= lookahead_distance) {
+      target = path.poses[i + 1];
+      break;
+    }
+  }
+
+  double dx = target.pose.position.x - odom.pose.pose.position.x;
+  double dy = target.pose.position.y - odom.pose.pose.position.y;
+  double target_distance = std::hypot(dx, dy);
+  double target_heading = std::atan2(dy, dx); 
+  
+  double qx = odom.pose.pose.orientation.x;
+  double qy = odom.pose.pose.orientation.y;
+  double qz = odom.pose.pose.orientation.z;
+  double qw = odom.pose.pose.orientation.w;
+  double heading = std::atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));
+  double d_heading = target_heading - heading;
+
+  // ensure change in heading is between -pi and pi
+  d_heading = std::atan2(std::sin(d_heading), std::cos(d_heading));
+
+  // TODO: tune speed
+  double angular_speed = std::clamp(d_heading * 2.0, -max_angular_speed, max_angular_speed);
+  double linear_speed = std::clamp(target_distance * 1.2, 0.0, max_linear_speed); 
+
+  if (is_near(goal.pose.position.x, goal.pose.position.y, 0.5)) {
+    linear_speed = 0;
+    angular_speed = 0;
+  } 
+
+  geometry_msgs::msg::Twist twist_msg;
+  twist_msg.linear.x = linear_speed; 
+  twist_msg.linear.y = 0; 
+  twist_msg.linear.z = 0;
+  twist_msg.angular.x = 0;
+  twist_msg.angular.y = 0;
+  twist_msg.angular.z = angular_speed;
+
+  cmd_vel_pub_->publish(twist_msg);
+}
+
+bool ControlNode::is_near(double target_x, double target_y, double tolerance) {
+  double dx = target_x - odom.pose.pose.position.x;
+  double dy = target_y - odom.pose.pose.position.y;
+  return std::hypot(dx, dy) <= tolerance;
+}
+
 
 int main(int argc, char ** argv)
 {
diff --git a/src/robot/costmap/CMakeLists.txt b/src/robot/costmap/CMakeLists.txt
index 130ebb88..ec281dbc 100644
--- a/src/robot/costmap/CMakeLists.txt
+++ b/src/robot/costmap/CMakeLists.txt
@@ -13,6 +13,9 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -25,7 +28,12 @@ add_library(costmap_lib
 target_include_directories(costmap_lib
   PUBLIC include)
 # Add ROS2 dependencies required by package
-ament_target_dependencies(costmap_lib rclcpp)
+target_link_libraries(costmap_lib PUBLIC
+  ${std_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  ${sensor_msgs_TARGETS}
+  rclcpp::rclcpp
+)
 
 # Create ROS2 node executable from source files
 add_executable(costmap_node src/costmap_node.cpp)
diff --git a/src/robot/costmap/include/costmap_core.hpp b/src/robot/costmap/include/costmap_core.hpp
index b8baa599..0c2e10fb 100644
--- a/src/robot/costmap/include/costmap_core.hpp
+++ b/src/robot/costmap/include/costmap_core.hpp
@@ -1,19 +1,20 @@
 #ifndef COSTMAP_CORE_HPP_
 #define COSTMAP_CORE_HPP_
 
+#include <vector>
+#include <queue>
+
 #include "rclcpp/rclcpp.hpp"
 
 namespace robot
 {
-
 class CostmapCore {
   public:
-    // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal
-    explicit CostmapCore(const rclcpp::Logger& logger);
+  // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal
+  explicit CostmapCore(const rclcpp::Logger& logger);
 
   private:
     rclcpp::Logger logger_;
-
 };
 
 }  
diff --git a/src/robot/costmap/include/costmap_node.hpp b/src/robot/costmap/include/costmap_node.hpp
index df7d5f1b..45f02e80 100644
--- a/src/robot/costmap/include/costmap_node.hpp
+++ b/src/robot/costmap/include/costmap_node.hpp
@@ -2,15 +2,42 @@
 #define COSTMAP_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp" 
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include <vector>
 
 #include "costmap_core.hpp"
 
 class CostmapNode : public rclcpp::Node {
+  struct Point {
+    int x, y;
+  };
+
   public:
     CostmapNode();
 
   private:
     robot::CostmapCore costmap_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_pub_;
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sensor_msg_sub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // costmap settings
+    // TODO: find most optimal constants for mapping
+    const int rows = 200;
+    const int cols = 200; 
+    const float resolution = 0.1;
+    const float width_m = cols * resolution;
+    const float height_m = rows * resolution;
+    const float inflation_radius_m = 2.0;
+    const float max_cost = 100;
+
+    std::vector<float> create_costmap(float angle_min, float angle_increment,
+      float range_min, float range_max, const std::vector<float> &ranges);
+    void sensor_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
 };
 
 #endif 
\ No newline at end of file
diff --git a/src/robot/costmap/package.xml b/src/robot/costmap/package.xml
index 4651c557..aa02bd49 100644
--- a/src/robot/costmap/package.xml
+++ b/src/robot/costmap/package.xml
@@ -10,6 +10,9 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/robot/costmap/src/costmap_node.cpp b/src/robot/costmap/src/costmap_node.cpp
index 33a47a77..7dbfb9c9 100644
--- a/src/robot/costmap/src/costmap_node.cpp
+++ b/src/robot/costmap/src/costmap_node.cpp
@@ -1,9 +1,105 @@
 #include <chrono>
 #include <memory>
+#include <cmath>
 
 #include "costmap_node.hpp"
 
-CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) {}
+CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) {
+  string_pub_ = create_publisher<std_msgs::msg::String>("/test_topic", 10);
+  costmap_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/costmap", 10);
+  sensor_msg_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+    "/lidar", 10, std::bind(&CostmapNode::sensor_callback, this, std::placeholders::_1));
+}
+
+void CostmapNode::sensor_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
+  auto m = msg.get();
+  auto costmap = create_costmap(m->angle_min, m->angle_increment, m->range_min, m->range_max, m->ranges);
+
+  nav_msgs::msg::OccupancyGrid nav_msg;
+  nav_msg.header.frame_id = "sim_world";
+  nav_msg.header.stamp = now();
+  nav_msg.info.width = cols;
+  nav_msg.info.height = rows;
+  nav_msg.info.resolution = resolution;
+  nav_msg.info.origin.position.x = -width_m / 2.0;
+  nav_msg.info.origin.position.y = -height_m / 2.0;
+  nav_msg.info.origin.position.z = 0;
+  nav_msg.info.origin.orientation.w = 1.0;
+  nav_msg.data.resize(rows * cols);
+
+  size_t n = costmap.size();
+  for (size_t i = 0; i < n; i++) {
+    nav_msg.data[i] = costmap[i]; 
+  }
+
+  costmap_pub_->publish(nav_msg);
+}
+
+//TODO: find a more efficient solution
+std::vector<float> CostmapNode::create_costmap(float angle_min, float angle_increment,
+  float range_min, float range_max, const std::vector<float> &ranges) {
+  std::vector<float> costmap(rows * cols, 0.0f);
+  std::queue<Point> obstacles;
+  size_t n = ranges.size();
+
+  // origin indices in grid
+  Point origin;
+  origin.x = std::floor(cols / 2);
+  origin.y = std::floor(rows / 2);
+
+  for (size_t i = 0; i < n; i++) {
+    double angle = angle_min + angle_increment * i;
+    double range = ranges[i];
+
+    if (range <= range_max && range >= range_min) {
+      // transform cartesian coordinates into grid indices
+      int x = origin.x + std::floor(range * std::cos(angle) / resolution);
+      int y = origin.y + std::floor(range * std::sin(angle) / resolution); 
+
+      if (x < 0 || x >= cols || y < 0 || y >= rows) {
+        //RCLCPP_INFO(get_logger(), "costmap index out of bound: (x=%d, y=%d)", x, y);
+        continue;
+      }
+
+      // mark obstacle
+      costmap[y * cols + x] = max_cost;  
+      // stores obstacle points
+      obstacles.push({x, y});
+    }
+  }
+
+  const int step_size = std::ceil(inflation_radius_m / resolution);
+
+  while (!obstacles.empty()) {
+    Point obstacle = obstacles.front();
+    obstacles.pop();
+
+    // calculate the euclidean distance between an obstacle cell and adjacent cells 
+    for (int dy = -step_size; dy <= step_size; dy++) {
+      for (int dx = -step_size; dx <= step_size; dx++) {
+        int adj_x = obstacle.x + dx;
+        int adj_y = obstacle.y + dy;
+
+        if (adj_x < 0 || adj_x >= cols || adj_y < 0 || adj_y >= rows) {
+          continue;
+        }
+
+        // calculate euclidean distance in meter
+        double distance_m = std::hypot(dx, dy) * resolution;
+
+        if (distance_m < inflation_radius_m) {
+          // assign inflated cost to the surrounding cell
+          float cost = max_cost * (1 - distance_m / inflation_radius_m);
+          if (costmap[adj_y * cols + adj_x] < cost) {
+            costmap[adj_y * cols + adj_x] = cost;
+          }
+        }
+      }
+    }
+  }
+
+  return costmap;
+}
 
 int main(int argc, char ** argv)
 {
diff --git a/src/robot/map_memory/CMakeLists.txt b/src/robot/map_memory/CMakeLists.txt
index 04458703..d4dd8690 100644
--- a/src/robot/map_memory/CMakeLists.txt
+++ b/src/robot/map_memory/CMakeLists.txt
@@ -13,6 +13,7 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(nav_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -25,8 +26,9 @@ add_library(map_memory_lib
 target_include_directories(map_memory_lib
   PUBLIC include)
 # Add ROS2 dependencies required by package
-ament_target_dependencies(map_memory_lib 
-  rclcpp
+target_link_libraries(map_memory_lib PUBLIC
+  rclcpp::rclcpp
+  ${nav_msgs_TARGETS}
 )
 
 # Create ROS2 node executable from source files
@@ -44,5 +46,4 @@ install(DIRECTORY
   config
   DESTINATION share/${PROJECT_NAME})
 
-
-ament_package()
+ament_package()
\ No newline at end of file
diff --git a/src/robot/map_memory/include/map_memory_node.hpp b/src/robot/map_memory/include/map_memory_node.hpp
index e55a567c..011dc7b0 100644
--- a/src/robot/map_memory/include/map_memory_node.hpp
+++ b/src/robot/map_memory/include/map_memory_node.hpp
@@ -2,15 +2,42 @@
 #define MAP_MEMORY_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 #include "map_memory_core.hpp"
 
 class MapMemoryNode : public rclcpp::Node {
   public:
     MapMemoryNode();
+    void publish();
 
   private:
     robot::MapMemoryCore map_memory_;
+    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    nav_msgs::msg::OccupancyGrid global_map_;
+    nav_msgs::msg::OccupancyGrid last_costmap_;
+    nav_msgs::msg::Odometry odom;
+    double last_x, last_y;
+    const double threshold_m = 1.5;
+    bool update_map = false;
+    bool costmap_updated = false;
+
+    // global map settings
+    const int cols = 300;
+    const int rows = 300; 
+    const float resolution = 0.1;
+    const float width_m = cols * resolution;
+    const float height_m = rows * resolution;
+    const double map_origin_x = -width_m / 2.0;
+    const double map_origin_y = -height_m / 2.0;
+
+    void costmap_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+    void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
 };
 
 #endif 
diff --git a/src/robot/map_memory/package.xml b/src/robot/map_memory/package.xml
index 9cc4b5fc..af6474d8 100644
--- a/src/robot/map_memory/package.xml
+++ b/src/robot/map_memory/package.xml
@@ -10,6 +10,7 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/robot/map_memory/src/map_memory_node.cpp b/src/robot/map_memory/src/map_memory_node.cpp
index 1ce77741..c7a5852d 100644
--- a/src/robot/map_memory/src/map_memory_node.cpp
+++ b/src/robot/map_memory/src/map_memory_node.cpp
@@ -1,6 +1,96 @@
 #include "map_memory_node.hpp"
 
-MapMemoryNode::MapMemoryNode() : Node("map_memory"), map_memory_(robot::MapMemoryCore(this->get_logger())) {}
+#include <vector>
+
+MapMemoryNode::MapMemoryNode() : Node("map_memory"), map_memory_(robot::MapMemoryCore(this->get_logger())) {
+  costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+    "/costmap",
+    10,
+    std::bind(&MapMemoryNode::costmap_callback, this, std::placeholders::_1));
+  odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+    "/odom/filtered",
+    10,
+    std::bind(&MapMemoryNode::odometry_callback, this, std::placeholders::_1));
+  map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/map", 10);
+  // run a periodic loop at 1s
+  timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&MapMemoryNode::publish, this));
+
+  // global map settings
+  global_map_.header.frame_id = "sim_world";
+  global_map_.info.origin.position.x = map_origin_x;
+  global_map_.info.origin.position.y = map_origin_y;
+  global_map_.info.origin.position.z = 0;
+  global_map_.info.width = cols; 
+  global_map_.info.height = rows; 
+  global_map_.info.resolution = resolution;
+  global_map_.data.resize(rows * cols, 0);
+}
+
+void MapMemoryNode::costmap_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+  last_costmap_ = *msg;
+  costmap_updated = true;
+}
+
+void MapMemoryNode::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+  auto m = msg.get();
+  odom = *m;
+  double x = m->pose.pose.position.x;
+  double y = m->pose.pose.position.y;
+  double dist = std::hypot(x - last_x, y - last_y);
+  if (dist >= threshold_m) {
+    last_x = x;
+    last_y = y;
+    update_map = true;
+  }
+}
+
+void MapMemoryNode::publish() {
+  if (!update_map || !costmap_updated) return;
+
+  const double pose_x = odom.pose.pose.position.x;
+  const double pose_y = odom.pose.pose.position.y;
+
+  double qx = odom.pose.pose.orientation.x;
+  double qy = odom.pose.pose.orientation.y;
+  double qz = odom.pose.pose.orientation.z;
+  double qw = odom.pose.pose.orientation.w;
+  // yaw
+  double heading = std::atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));
+
+  const int n = (int) last_costmap_.data.size();
+  const int costmap_cols = last_costmap_.info.width;
+  const int costmap_rows = last_costmap_.info.height; 
+  const float costmap_resolution = last_costmap_.info.resolution;
+
+  for (size_t i = 0; i < n; i++) {
+    // costmap cell index
+    int ix = i % costmap_cols;
+    int iy = i / costmap_cols;
+    // convert the cell index into pose in robot frame 
+    double local_x = last_costmap_.info.origin.position.x + (ix + 0.5) * costmap_resolution;
+    double local_y = last_costmap_.info.origin.position.y + (iy + 0.5) * costmap_resolution;
+    // convert robot frame cell pose into global frame: rotate by heading and translate by robot pose
+    double global_x = pose_x + local_x * std::cos(heading) - local_y * std::sin(heading);
+    double global_y = pose_y + local_x * std::sin(heading) + local_y * std::cos(heading);
+    // convert the global cell pose into global map index
+    int gx = std::floor((global_x - global_map_.info.origin.position.x) / global_map_.info.resolution);
+    int gy = std::floor((global_y - global_map_.info.origin.position.y) / global_map_.info.resolution);
+
+    // out of bound
+    if (gx < 0 || gx >= cols || gy < 0 || gy >= rows) {
+      continue;
+    }
+
+    int8_t cost = last_costmap_.data[i]; 
+    if (cost <= 100 && cost >= 0) {
+      global_map_.data[gy * cols + gx] = cost; 
+    }
+  }
+
+  global_map_.header.stamp = now();
+  map_pub_->publish(global_map_);
+  update_map = false;
+}
 
 int main(int argc, char ** argv)
 {
diff --git a/src/robot/odometry_spoof/CMakeLists.txt b/src/robot/odometry_spoof/CMakeLists.txt
index c236ab10..09e923dd 100644
--- a/src/robot/odometry_spoof/CMakeLists.txt
+++ b/src/robot/odometry_spoof/CMakeLists.txt
@@ -24,13 +24,13 @@ add_executable(odometry_spoof src/odometry_spoof.cpp)
 target_include_directories(odometry_spoof
   PUBLIC include)
 # Add ROS2 dependencies required by package
-ament_target_dependencies(odometry_spoof
-  rclcpp
-  geometry_msgs
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  nav_msgs
+target_link_libraries(odometry_spoof
+  rclcpp::rclcpp
+  ${geometry_msgs_TARGETS}
+  tf2::tf2
+  tf2_ros::tf2_ros
+  ${tf2_geometry_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
 )
 
 # Copy executable to installation location
diff --git a/src/robot/planner/CMakeLists.txt b/src/robot/planner/CMakeLists.txt
index af32e490..95fd4239 100644
--- a/src/robot/planner/CMakeLists.txt
+++ b/src/robot/planner/CMakeLists.txt
@@ -13,6 +13,8 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -25,7 +27,11 @@ add_library(planner_lib
 target_include_directories(planner_lib
   PUBLIC include)
 # Add ROS2 dependencies required by package
-ament_target_dependencies(planner_lib rclcpp)
+target_link_libraries(planner_lib 
+  rclcpp::rclcpp
+  ${nav_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
+)
 
 # Create ROS2 node executable from source files
 add_executable(planner_node src/planner_node.cpp)
diff --git a/src/robot/planner/include/node.hpp b/src/robot/planner/include/node.hpp
new file mode 100644
index 00000000..bdb8224f
--- /dev/null
+++ b/src/robot/planner/include/node.hpp
@@ -0,0 +1,61 @@
+#ifndef aNODE_HPP_
+#define aNODE_HPP_
+
+#include <functional>
+#include <memory>
+#include <cmath>
+
+// either used to represent cell index or pose
+struct Point {
+    double x, y;
+    Point(double x_, double y_) : x(x_), y(y_) {};
+    Point() : x(0), y(0) {};
+
+    double distance_to(Point other) {
+        return std::hypot(x - other.x, y - other.y);
+    }
+
+    bool operator==(const Point &other) const
+    {
+        return (x == other.x && y == other.y);
+    }
+
+    struct Hash {
+        std::size_t operator()(const Point &idx) const {
+            return std::hash<int>()(idx.x) ^ (std::hash<int>()(idx.y) << 1);
+        }
+    };
+};
+
+
+class aNode {
+    public:
+    Point point;
+    double f = -1; // f = g + h
+    double g = -1; // cost from start to this node 
+    double h = -1; // herustic distance to goal
+    aNode *parent = nullptr;
+
+    aNode() : point() {}
+    aNode(int x, int y) : point(x, y) {}
+    aNode(const Point& p) : point(p) {}
+    aNode(const Point& p, double f_, double g_, double h_) 
+        : point(p), f(f_), g(g_), h(h_) {}
+
+    bool operator==(const aNode& other) const {
+        return point == other.point;
+    }
+
+    struct CompareNode {
+        bool operator()(const aNode *a, const aNode *b) const {
+            return a->f > b->f;
+        }
+    };
+
+    struct Hash {
+        std::size_t operator()(const aNode& n) const {
+            return std::hash<int>()(n.point.x) ^ (std::hash<int>()(n.point.y) << 1);
+        }
+    };
+};
+#endif
\ No newline at end of file
diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner_node.hpp
index 95562ed2..5297ee55 100644
--- a/src/robot/planner/include/planner_node.hpp
+++ b/src/robot/planner/include/planner_node.hpp
@@ -1,9 +1,24 @@
 #ifndef PLANNER_NODE_HPP_
 #define PLANNER_NODE_HPP_
 
+#include <queue>
+#include <unordered_map>
+#include <unordered_set>
+#include <stack>
+
 #include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
 
 #include "planner_core.hpp"
+#include "node.hpp"
+
+enum State {
+  WAITING_FOR_GOAL,
+  REACHING_TO_GOAL
+};
 
 class PlannerNode : public rclcpp::Node {
   public:
@@ -11,6 +26,29 @@ class PlannerNode : public rclcpp::Node {
 
   private:
     robot::PlannerCore planner_;
+    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr goal_sub_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    State state_;
+    nav_msgs::msg::OccupancyGrid global_map_;
+    geometry_msgs::msg::PointStamped goal_;
+    geometry_msgs::msg::PoseWithCovariance robot_pose_;
+    bool goal_updated = false;
+    bool planning = false;
+
+    void plan_path();
+    void publish_path();
+    bool is_close(Point point, Point target, double threshold);
+    bool is_valid(Point cell_index);
+    Point pose_to_index(Point pose);
+
+    void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+    void goal_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg);
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+    void timer_callback();
 };
 
 #endif 
diff --git a/src/robot/planner/package.xml b/src/robot/planner/package.xml
index b29cb4ed..4e47e378 100644
--- a/src/robot/planner/package.xml
+++ b/src/robot/planner/package.xml
@@ -10,6 +10,8 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/robot/planner/src/planner_node.cpp b/src/robot/planner/src/planner_node.cpp
index ed5a3b75..7dd26067 100644
--- a/src/robot/planner/src/planner_node.cpp
+++ b/src/robot/planner/src/planner_node.cpp
@@ -1,6 +1,192 @@
 #include "planner_node.hpp"
 
-PlannerNode::PlannerNode() : Node("planner"), planner_(robot::PlannerCore(this->get_logger())) {}
+PlannerNode::PlannerNode() : Node("planner"), 
+  planner_(robot::PlannerCore(this->get_logger())), state_(State::WAITING_FOR_GOAL) {
+  map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      "/map", 10, std::bind(&PlannerNode::map_callback, this, std::placeholders::_1));
+  goal_sub_ = create_subscription<geometry_msgs::msg::PointStamped>(
+      "/goal_point", 10, std::bind(&PlannerNode::goal_callback, this, std::placeholders::_1));
+  odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+      "/odom/filtered", 10, std::bind(&PlannerNode::odom_callback, this, std::placeholders::_1));
+  path_pub_ = create_publisher<nav_msgs::msg::Path>("/path", 10);
+  timer_ = create_wall_timer(std::chrono::milliseconds(500), std::bind(&PlannerNode::timer_callback, this));
+}
+
+void PlannerNode::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+  global_map_ =  *msg; 
+  if (state_ == State::REACHING_TO_GOAL) {
+    plan_path();
+  }
+}
+
+void PlannerNode::goal_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) {  
+  goal_ = *msg;
+  goal_updated = true;
+  state_ = State::REACHING_TO_GOAL;
+  plan_path();
+}
+
+void PlannerNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+  robot_pose_ = msg->pose;
+}
+
+void PlannerNode::timer_callback() {
+  Point robot_p;
+  robot_p.x = robot_pose_.pose.position.x;
+  robot_p.y = robot_pose_.pose.position.y;
+  Point goal_p;
+  goal_p.x = goal_.point.x;
+  goal_p.y = goal_.point.y;
+
+  if (state_ == State::REACHING_TO_GOAL) {
+    if (is_close(robot_p, goal_p, 0.5)) {
+      RCLCPP_INFO(this->get_logger(), "Goal reached!");
+      state_ = State::WAITING_FOR_GOAL;
+    } else {
+      RCLCPP_INFO(this->get_logger(), "Replanning due to timeout or progress...");
+      plan_path();
+    }
+  }
+}
+
+// find the shortest path to goal using A* search
+void PlannerNode::plan_path() {
+  if (!goal_updated || global_map_.data.empty()) {
+    RCLCPP_WARN(this->get_logger(), "Cannot plan path: Missing map or goal!");
+    return;
+  }
+
+  std::priority_queue<aNode*, std::vector<aNode*>, aNode::CompareNode> to_visit;
+  std::unordered_map<Point, aNode*, Point::Hash> in_queue; // used to check if a node exists in to_visit queue
+  std::unordered_set<Point, Point::Hash> visited;
+  std::stack<Point> path;
+
+  // convert start and goal pose into global map cell indices
+  Point start;
+  start.x = robot_pose_.pose.position.x;
+  start.y = robot_pose_.pose.position.y;
+  start = pose_to_index(start);
+  Point goal;
+  goal.x = goal_.point.x;
+  goal.y = goal_.point.y;
+  goal = pose_to_index(goal);
+
+  //RCLCPP_INFO(this->get_logger(), "Start: (%f, %f), Goal: (%f, %f)", start.x, start.y, goal.x, goal.y);
+  
+  double g = global_map_.data[start.y * global_map_.info.width + start.x];
+  double h = start.distance_to(goal); 
+  double f = h + g;
+  aNode *start_node = new aNode(start, f, g, h);
+
+  to_visit.push(start_node);
+  in_queue.insert({start, start_node});
+
+  while (!to_visit.empty()) {
+    // get node with the smallest f value
+    aNode *min_node = to_visit.top();
+    to_visit.pop();
+    visited.insert(min_node->point);
+
+    if (min_node->point == goal) {
+      // create path by iterating nodes reversely
+      // make sure the end point is the goal point
+      aNode *current = min_node;
+      while (current != nullptr) {
+        path.push(current->point);
+        current = current->parent;
+      }
+      break;
+    }
+
+    // check for adjacent nodes
+    for (int dx = -1; dx <= 1; dx++) {
+      for (int dy = -1; dy <= 1; dy++) {
+        // don't check itself
+        if (dx == 0 && dy == 0) continue;
+
+        Point adj_point;
+        adj_point.x = min_node->point.x + dx;
+        adj_point.y = min_node->point.y + dy;
+
+        if (visited.find(adj_point) != visited.end() || !is_valid(adj_point)) {
+          continue;
+        }
+
+        double g; // a cell is always one unit next to its adjacent cell.
+
+        if (dx != 0 && dy != 0) {
+          g = min_node->g + std::sqrt(2.0); // for cell located diagonally
+        } else {
+          g = min_node->g + 1.0;
+        }
+
+        g += global_map_.data[adj_point.y * global_map_.info.width + adj_point.x];
+
+        double h = adj_point.distance_to(goal);
+        double f = h + g;
+
+        aNode *adj_node;
+        if (in_queue.find(adj_point) == in_queue.end()) {
+          // if adj_node is not in to_visit queue
+          // create a new adjacent node
+          adj_node = new aNode(adj_point, f, g, h);
+          adj_node->parent = min_node; 
+          to_visit.push(adj_node);
+          in_queue.insert({adj_point, adj_node});
+        } else {
+          // if adj_node exists in to_visit queue
+          // override the existing adjacent node
+          adj_node = in_queue[adj_point];
+          // if found a better path, update
+          if (g < adj_node->g) {
+            adj_node->g = g;
+            adj_node->h = h;
+            adj_node->f = f;
+            adj_node->parent = min_node;
+            // priority queue does not re-adjust automatically
+            // so insert it again
+            to_visit.push(adj_node); 
+          }
+        }
+      }
+    }
+  }
+  
+  // publish path
+  nav_msgs::msg::Path p;
+  p.header.frame_id = "sim_world";
+  p.header.stamp = get_clock()->now();
+  while (!path.empty()) {
+    Point point = path.top();
+    path.pop();
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = "sim_world";
+    // convert global index into global pose
+    pose.pose.position.x = point.x * global_map_.info.resolution + global_map_.info.origin.position.x;
+    pose.pose.position.y = point.y * global_map_.info.resolution + global_map_.info.origin.position.y;
+    p.poses.push_back(pose);
+  }
+  path_pub_->publish(p);
+}
+
+// converts pose into global map cell index
+Point PlannerNode::pose_to_index(Point pose) {
+  Point p;
+  p.x = std::floor((pose.x - global_map_.info.origin.position.x) / global_map_.info.resolution);
+  p.y = std::floor((pose.y - global_map_.info.origin.position.y) / global_map_.info.resolution);
+  return p;
+}
+
+bool PlannerNode::is_valid(Point cell_index) {
+  return cell_index.x < global_map_.info.width && cell_index.y < global_map_.info.height 
+    && cell_index.x >= 0 && cell_index.y >= 0;
+}
+
+bool PlannerNode::is_close(Point point, Point target, double threshold) {
+  double dx = point.x - target.x;
+  double dy = point.y - target.y;
+  return std::hypot(dx, dy) <= threshold;
+}
 
 int main(int argc, char ** argv)
 {
@@ -8,4 +194,4 @@ int main(int argc, char ** argv)
   rclcpp::spin(std::make_shared<PlannerNode>());
   rclcpp::shutdown();
   return 0;
-}
+}
\ No newline at end of file
diff --git a/watod-config.sh b/watod-config.sh
index bc5da50a..513c0399 100755
--- a/watod-config.sh
+++ b/watod-config.sh
@@ -15,7 +15,7 @@
 ##   - robot            :   starts up robot nodes
 ##   - samples          :   starts up sample nodes for reference
 
-# ACTIVE_MODULES=""
+ACTIVE_MODULES="robot gazebo vis_tools"
 
 ################################# MODE OF OPERATION #################################
 ## Possible modes of operation when running watod.
@@ -38,4 +38,4 @@
 
 ## Platform in which to build the docker images with. 
 ## Either arm64 (apple silicon, raspberry pi) or amd64 (most computers)
-# PLATFORM="amd64"
+PLATFORM="amd64"
\ No newline at end of file