diff --git a/.github/workflows/foxy_build.yaml b/.github/workflows/foxy_build.yaml
index 45ac280..972384b 100644
--- a/.github/workflows/foxy_build.yaml
+++ b/.github/workflows/foxy_build.yaml
@@ -2,8 +2,8 @@ name: ROS 2 Foxy
 
 on:
   push:
-    branches: 
-      - "feature/ros2"
+    branches:
+      - feature/ros2
 
 jobs:
   build:
@@ -11,11 +11,10 @@ jobs:
     container:
       image: ros:foxy
     steps:
-      - name: Create Workspace
-        run: mkdir -p $HOME/workspaces/s_graphs_ros2_ws/src     
-     
-      - name: Checkout code
-        run: cd $HOME/workspaces/s_graphs_ros2_ws/src && . /opt/ros/foxy/setup.sh  && git clone https://github.com/snt-arg/s_graphs.git
-      
+      - name: Create Workspace and checkout code
+        uses: actions/checkout@v3
+        with:
+          submodules: recursive
+
       - name: Build
-        run: . /opt/ros/foxy/setup.sh && ./setup.sh
\ No newline at end of file
+        run: . /opt/ros/foxy/setup.sh && ./.github/workflows/setup.sh && colcon build
\ No newline at end of file
diff --git a/.github/workflows/humble_build.yaml b/.github/workflows/humble_build.yaml
index 88ab758..c55292e 100644
--- a/.github/workflows/humble_build.yaml
+++ b/.github/workflows/humble_build.yaml
@@ -13,13 +13,8 @@ jobs:
     steps:
       - name: Create Workspace and checkout code
         uses: actions/checkout@v3
+        with:
+          submodules: recursive
 
       - name: Build
-        run: |
-          . /opt/ros/humble/setup.sh && \
-          mkdir -p $HOME/workspace/s_graphs_ws/src && \
-          cd .. && \
-          cp -r s_graphs $HOME/workspace/s_graphs_ws/src && \
-          cd $HOME/workspace/s_graphs_ws/src/s_graphs && \
-          ./.github/workflows/setup.sh && \
-          cd ../../ && colcon build --symlink-install
\ No newline at end of file
+        run: . /opt/ros/humble/setup.sh && ./.github/workflows/setup.sh && colcon build
\ No newline at end of file
diff --git a/.github/workflows/iron_build.yaml b/.github/workflows/iron_build.yaml
index 02d9cc3..9ef70bc 100644
--- a/.github/workflows/iron_build.yaml
+++ b/.github/workflows/iron_build.yaml
@@ -3,7 +3,7 @@ name: ROS 2 Iron
 on:
   push:
     branches:
-     - feature/ros2
+      - feature/ros2
 
 jobs:
   build:
@@ -11,11 +11,10 @@ jobs:
     container:
       image: ros:iron
     steps:
-      - name: Create Workspace
-        run: mkdir -p $HOME/workspaces/s_graphs_ros2_ws/src && cd $HOME/workspaces/s_graphs_ros2_ws/src && . /opt/ros/iron/setup.sh
-
-      - name: Checkout code
-        run: cd $HOME/workspaces/s_graphs_ros2_ws/src && . /opt/ros/iron/setup.sh  && git clone https://github.com/snt-arg/s_graphs.git
+      - name: Create Workspace and checkout code
+        uses: actions/checkout@v3
+        with:
+          submodules: recursive
 
       - name: Build
-        run: . /opt/ros/iron/setup.sh && ./setup.sh
\ No newline at end of file
+        run: . /opt/ros/iron/setup.sh && ./.github/workflows/setup.sh && colcon build
\ No newline at end of file
diff --git a/.github/workflows/setup.sh b/.github/workflows/setup.sh
index b2c4319..801efbe 100755
--- a/.github/workflows/setup.sh
+++ b/.github/workflows/setup.sh
@@ -1,10 +1,19 @@
 #!/bin/bash
 
-# - Installing cpp dependencies
 sudo apt update
+sudo apt install -y libceres-dev
+
+#rosdep init
+rosdep update --include-eol-distros
 
 #import all repos
-vcs import --recursive < .rosinstall_ros2
+git submodule update --init --recursive
+
+#install python requirements
+pip3 install -r requirements.txt
 
 #rosdep install
-rosdep install --from-paths . -y --ignore-src -r
\ No newline at end of file
+rosdep install --from-paths . -y --ignore-src -r
+
+# - Importing all dependencies
+colcon build --symlink-install
\ No newline at end of file
diff --git a/.real_mprocs.yaml b/.real_mprocs.yaml
index 32c4cfd..0d60166 100644
--- a/.real_mprocs.yaml
+++ b/.real_mprocs.yaml
@@ -1,11 +1,11 @@
 procs:
   rosbag:
-    shell: 'bash -c "source /opt/ros/foxy/setup.bash && rosbag play ~/Downloads/real/stugalux_oetrange_f2_3r.db3"'
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && ros2 bag play ~/Downloads/real/stugalux_oetrange_f2_3r.db3"'
     autostart: true
     stop: SIGINT
   s_graphs:
-    shell: 'bash -c "source ~/workspaces/s_graphs_ros2_ws/install/setup.bash && ros2 launch s_graphs s_graphs_launch.py compute_odom:=false env:=real"'
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && ros2 launch s_graphs s_graphs_launch.py compute_odom:=false env:=real"'
     autostart: true
   rviz:
-    shell: 'bash -c "source ~/workspaces/s_graphs_ros2_ws/install/setup.bash && rviz2 -d ~/workspaces/s_graphs_ros2_ws/src/s_graphs/rviz/s_graphs_ros2.rviz"'
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && rviz2 -d ~/workspaces/s_graphs/s_graphs/rviz/s_graphs_ros2.rviz"'
     autostart: true
diff --git a/.virtual_mprocs.yaml b/.virtual_mprocs.yaml
index 9459b17..faca665 100644
--- a/.virtual_mprocs.yaml
+++ b/.virtual_mprocs.yaml
@@ -1,11 +1,11 @@
 procs:
   rosbag:
-    shell: 'bash -c "source /opt/ros/foxy/setup.bash && ros2 bag play ~/Downloads/virtual/stugalux_oetrange_f2_3r.db3"'
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && ros2 bag play ~/Downloads/virtual/stugalux_oetrange_f2_3r.db3"'
     autostart: true
     stop: SIGINT
   s_graphs:
-    shell: 'bash -c "source ~/workspaces/s_graphs_ros2_ws/install/setup.bash && ros2 launch s_graphs s_graphs_launch.py compute_odom:=true env:=sim"'
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && ros2 launch s_graphs s_graphs_launch.py compute_odom:=true env:=sim"'
     autostart: true
   rviz:
-    shell: 'bash -c "source ~/workspaces/s_graphs_ros2_ws/install/setup.bash && rviz2 -d ~/workspaces/s_graphs_ros2_ws/src/s_graphs/rviz/s_graphs_ros2.rviz"'
-    autostart: true
+    shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && rviz2 -d ~/workspaces/s_graphs/s_graphs/rviz/s_graphs_ros2.rviz"'
+    autostart: true
\ No newline at end of file