diff --git a/config/ouster_jackal_default.yaml b/config/ouster_jackal_default.yaml
index e9b4b08d..28aea4e8 100644
--- a/config/ouster_jackal_default.yaml
+++ b/config/ouster_jackal_default.yaml
@@ -55,6 +55,7 @@
       origin_lng: -79.4661
       origin_theta: 1.3
       scale: 1.0
+      gps_topic: "/novatel/fix"
     tactic:
       enable_parallelization: true
       preprocessing_skippable: false
diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalForm.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalForm.js
index 5de5531f..6eb59062 100644
--- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalForm.js
+++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/goal/GoalForm.js
@@ -43,7 +43,7 @@ class GoalForm extends React.Component {
   }
 
   render() {
-    const { waypointsMap, goalType, goalWaypoints, setNewGoalWaypoints } = this.props;
+    const { goalType, goalWaypoints, setNewGoalWaypoints } = this.props;
     const { goal_form_open, goal_waypoints_str, goal_waypoints_invalid, pause_after, pause_before } = this.state;
     return (
       <>
diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
index 7fd8b6e0..f81a022c 100644
--- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
+++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
@@ -20,7 +20,7 @@ import React from "react";
 import "leaflet/dist/leaflet.css";
 import L from "leaflet";
 import "leaflet-rotatedmarker"; // enable marker rotation
-import { Card, IconButton, TextField } from "@mui/material";
+import { Card, IconButton, TextField, Switch, Box, FormControlLabel, Typography } from "@mui/material";
 import { MapContainer, TileLayer, ZoomControl, ImageOverlay, Marker, Popup } from "react-leaflet";
 import { kdTree } from "kd-tree-javascript";
 
@@ -31,7 +31,6 @@ import GoalManager from "../goal/GoalManager";
 import TaskQueue from "../task_queue/TaskQueue";
 
 import NewGoalWaypointSVG from "../../images/new-goal-waypoint.svg";
-import RunningGoalWaypointSVG from "../../images/running-goal-waypoint.svg";
 import RobotIconSVG from "../../images/arrow.svg";
 import TargetIconSVG from "../../images/arrow-merge.svg";
 import SelectorCenterSVG from "../../images/selector-center.svg";
@@ -47,9 +46,12 @@ import AddIcon from "@mui/icons-material/Add";
 import DeleteIcon from "@mui/icons-material/Delete";
 
 /// pose graph constants
-const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000"];
+const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000", "#000000"];
+const ROUTE_TYPE_WIDTH = [0.4, 1.0, 2.0, 3.0, 4.0, 5.0, 20.0, 0.05];
+const ROUTE_TYPE_OPACITY = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.9];
+const ANNOTATE_LINE_COLOR = "#000000";
+const ANNOTATE_LINE_WIDTH = 0.5;
 const GRAPH_OPACITY = 0.9;
-const GRAPH_WEIGHT = 7;
 
 /// robot constants
 const ROBOT_OPACITY = 0.8;
@@ -73,11 +75,6 @@ const NEW_GOAL_WAYPOINT_ICON = new L.Icon({
   iconAnchor: [15, 30],
   iconSize: new L.Point(30, 30),
 });
-const RUNNING_GOAL_WAYPOINT_ICON = new L.Icon({
-  iconUrl: RunningGoalWaypointSVG,
-  iconAnchor: [15, 30],
-  iconSize: new L.Point(30, 30),
-});
 const WAYPOINT_OPACITY = 0.9;
 
 /// selector constants
@@ -143,6 +140,9 @@ class GraphMap extends React.Component {
       move_robot_vertex: { lng: 0, lat: 0, id: -1 },
       // map center
       map_center: {lat: 43.78220, lng: -79.4661},
+      /// whether the path selection will be the whole path (for annotation)
+      annotate_full: false,
+      in_select_mode: false,
     };
     this.fetchMapCenter()
     /// leaflet map
@@ -309,7 +309,6 @@ class GraphMap extends React.Component {
     const {
       server_state,
       waypoints_map,
-      display_waypoints_map,
       goals,
       curr_goal_idx,
       new_goal_type,
@@ -366,6 +365,31 @@ class GraphMap extends React.Component {
           // move robot
           moveRobotVertex={move_robot_vertex}
         />
+        <Box
+          sx={{
+            position: "absolute",
+            bottom: -3,
+            left: "72.5%",
+            transform: "translate(-50%, -50%)",
+            zIndex: 1000,
+            display: "flex",
+            flexDirection: "row",
+          }}
+        >
+          <FormControlLabel control={
+            <Switch
+              checked={this.state.annotate_full}
+              disabled={this.state.in_select_mode}
+              onChange={() => this.setState({annotate_full: !this.state.annotate_full})}
+              inputProps={{ 'aria-label': 'controlled' }}
+            />
+          } label={
+            <Typography sx={{ color: "#ffffff" }}>
+              SELECT FULL PATH
+            </Typography>
+          }
+          />
+        </Box>
         <GoalManager
           socket={socket}
           currentTool={current_tool}
@@ -410,6 +434,25 @@ class GraphMap extends React.Component {
       });
   }
 
+  zoomEnd = () => {
+    this.fixed_routes.forEach((route) => {
+      route.polyline.setStyle({ 
+        color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
+        weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
+        opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
+        lineCap: "butt",
+      })
+    });
+    this.active_routes.forEach((route) => {
+      route.polyline.setStyle({ 
+        color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
+        weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
+        opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
+        lineCap: "butt",
+      })
+    });
+  }
+
   /** @brief Leaflet map creationg callback */
   mapCreatedCallback(map) {
     console.debug("Leaflet map created.");
@@ -420,6 +463,8 @@ class GraphMap extends React.Component {
     //
     this.map.on("click", this.handleMapClick.bind(this));
     //
+    this.map.on("zoomend", this.zoomEnd, this);
+    //
     this.fetchGraphState(true);
   }
 
@@ -469,7 +514,6 @@ class GraphMap extends React.Component {
       if (this.state.new_goal_type === "repeat") {
         this.setState({ new_goal_waypoints: [...this.state.new_goal_waypoints, best.target.id] });
       }
-      console.log(this.state.waypoints_map);
     }
   }
 
@@ -486,37 +530,42 @@ class GraphMap extends React.Component {
   fetchGraphState(center = false) {
     console.info("Fetching the current pose graph state (full).");
     fetchWithTimeout("/vtr/graph")
-      .then((response) => {
-        if (response.status !== 200) throw new Error("Failed to fetch pose graph state: " + response.status);
-        response.json().then((data) => {
-          console.info("Received the pose graph state (full): ", data);
-          this.loadGraphState(data, center);
-          // fetch robot and following route after graph has been set
-          this.fetchRobotState();
-          this.fetchFollowingRoute();
-          this.fetchServerState();
-        });
-      })
-      .catch((error) => {
-        console.error(error);
+    .then((response) => {
+      if (response.status !== 200) throw new Error("Failed to fetch pose graph state: " + response.status);
+      response.json().then((data) => {
+        console.info("Received the pose graph state (full): ", data);
+        this.loadGraphState(data, center);
+        // fetch robot and following route after graph has been set
+        this.fetchRobotState();
+        this.fetchFollowingRoute();
+        this.fetchServerState();
       });
+    })
+    .catch((error) => {
+      console.error(error);
+    });
   }
-
+    
   graphStateCallback(graph_state) {
     console.info("Received graph state: ", graph_state);
     this.loadGraphState(graph_state);
   }
-
+  
   /** @brief Helper function to convert a pose graph route to a leaflet polyline, and add it to map */
   route2Polyline(route) {
     // fixed_routes format: [{type: 0, ids: [id, ...]}, ...]
     let color = ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length];
-    let width = route.type + 2
     let latlngs = route.ids.map((id) => {
       let v = this.id2vertex.get(id);
       return [v.lat, v.lng];
     });
-    let polyline = L.polyline(latlngs, { color: color, opacity: GRAPH_OPACITY, weight: width, pane: "graph" });
+    let polyline = L.polyline(latlngs, { 
+      color: color,
+      weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
+      opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
+      pane: "graph",
+      lineCap: "butt",
+    });
     polyline.addTo(this.map);
     return polyline;
   }
@@ -538,24 +587,30 @@ class GraphMap extends React.Component {
         wps_map.set(v.id, v.name);
       }
     });
-    this.setState({waypoints_map: wps_map, display_waypoints_map: wps_map}); 
+    this.setState({waypoints_map: wps_map, display_waypoints_map: wps_map});
 
     this.kdtree = new kdTree(graph.vertices, (a, b) => b.distanceTo(a), ["lat", "lng"]);
     // fixed routes
     this.fixed_routes.forEach((route) => {
       route.polyline.remove();
     });
-    this.fixed_routes = graph.fixed_routes.map((route) => {
+    this.fixed_routes = graph.fixed_routes.flatMap((route) => {
       let polyline = this.route2Polyline(route);
-      return { ...route, polyline: polyline };
+      let route_centre = structuredClone(route);
+      route_centre.type = 7;
+      let polyline_centre = this.route2Polyline(route_centre);
+      return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
     });
     // active routes
     this.active_routes.forEach((route) => {
       route.polyline.remove();
     });
-    this.active_routes = graph.active_routes.map((route) => {
+    this.active_routes = graph.active_routes.flatMap((route) => {
       let polyline = this.route2Polyline(route);
-      return { ...route, polyline: polyline };
+      let route_centre = structuredClone(route);
+      route_centre.type = 7;
+      let polyline_centre = this.route2Polyline(route_centre);
+      return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
     });
 
     // center set to root vertex
@@ -730,14 +785,23 @@ class GraphMap extends React.Component {
       let active_route = { ids: [vf.id], type: vf.type };
       active_route = { ...active_route, polyline: this.route2Polyline(active_route) };
       this.active_routes.push(active_route);
+      let active_route_centre = { ids: [vf.id], type: 7 };
+      active_route_centre = { ...active_route_centre, polyline: this.route2Polyline(active_route_centre) };
+      this.active_routes.push(active_route_centre);
     }
-    let active_route = this.active_routes[this.active_routes.length - 1];
+    let active_route = this.active_routes[this.active_routes.length - 2];
     active_route.ids.push(vt.id);
     active_route.polyline.addLatLng([vt.lat, vt.lng]);
+    let active_route_centre = this.active_routes[this.active_routes.length - 1];
+    active_route_centre.ids.push(vt.id);
+    active_route_centre.polyline.addLatLng([vt.lat, vt.lng]);
     if (active_route.type !== vt.type) {
       let new_active_route = { ids: [vt.id], type: vt.type };
       new_active_route = { ...new_active_route, polyline: this.route2Polyline(new_active_route) };
       this.active_routes.push(new_active_route);
+      let new_active_route_centre = { ids: [vf.id], type: 7 };
+      new_active_route_centre = { ...new_active_route_centre, polyline: this.route2Polyline(new_active_route_centre) };
+      this.active_routes.push(new_active_route_centre);
     }
   }
 
@@ -906,9 +970,17 @@ class GraphMap extends React.Component {
     let interm_pos = { s: null, c: null, e: null };
 
     let getSelectedPath = () => {
-      let paths = this.breadthFirstSearch(selector.vertex.c.id, [selector.vertex.s.id, selector.vertex.e.id]);
-      paths[0].reverse();
-      return paths[0].concat(paths[1].slice(1));
+      if (!this.state.annotate_full) {
+        let paths = this.breadthFirstSearch(selector.vertex.c.id, [selector.vertex.s.id, selector.vertex.e.id]);
+        paths[0].reverse();
+        return paths[0].concat(paths[1].slice(1));
+      }
+      else {
+        let end_id = Math.max(...Array.from(this.id2vertex.keys()));
+        let paths = this.breadthFirstSearch(selector.vertex.c.id, [0, end_id]);
+        paths[0].reverse();
+        return paths[0].concat(paths[1].slice(1));
+      }
     };
 
     let getRotationAngle = (p1, p2) => {
@@ -969,6 +1041,7 @@ class GraphMap extends React.Component {
     // set up and add the center marker
     let closest_vertices = this.kdtree.nearest(center_pos, 1);
     selector.vertex.c = closest_vertices ? closest_vertices[0][0] : center_pos;
+    selector.vertex.c = this.id2vertex.get(0);
     selector.marker.c = L.marker(selector.vertex.c, {
       draggable: true,
       icon: SELECTOR_CENTER_ICON,
@@ -1068,6 +1141,7 @@ class GraphMap extends React.Component {
   /// Annotate Route
   startAnnotateRoute() {
     console.info("Start annotating route");
+    this.setState({in_select_mode: true});
     this.annotate_route_selector = { marker: { s: null, c: null, e: null }, vertex: { s: null, c: null, e: null } };
     this.annotate_polyline = null;
     let selectPathCallback = (ids) => {
@@ -1078,10 +1152,11 @@ class GraphMap extends React.Component {
         });
         if (this.annotate_polyline === null) {
           this.annotate_polyline = L.polyline(latlngs, {
-            color: ROUTE_TYPE_COLOR[0],
+            color: ANNOTATE_LINE_COLOR,
             opacity: GRAPH_OPACITY,
-            weight: GRAPH_WEIGHT,
+            weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
             pane: "graph",
+            lineCap: "butt",
           });
           this.annotate_polyline.addTo(this.map);
         } else {
@@ -1090,10 +1165,28 @@ class GraphMap extends React.Component {
       });
     };
     this.createSelector(this.annotate_route_selector, selectPathCallback);
+
+    this.annotateRouteZoomEnd = () => {
+
+      let type = this.state.annotate_route_type;
+      this.setState({ annotate_route_type: type }, () =>
+        this.annotate_polyline.setStyle({ 
+          color: ANNOTATE_LINE_COLOR,
+          opacity: GRAPH_OPACITY,
+          weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
+          pane: "graph",
+          lineCap: "butt",
+        })
+      );
+    };
+    this.map.on("zoomend", this.annotateRouteZoomEnd, this);
   }
 
   finishAnnotateRoute() {
     console.info("Finish annotating route");
+    this.map.off("zoomend", this.annotateRouteZoomEnd, this);
+    this.moveGraphZoomEnd = null;
+
     if (this.annotate_polyline !== null) {
       this.annotate_polyline.remove();
       this.annotate_polyline = null;
@@ -1102,12 +1195,19 @@ class GraphMap extends React.Component {
       this.removeSelector(this.annotate_route_selector);
       this.annotate_route_selector = null;
     }
+    this.setState({in_select_mode: false});
   }
 
   handleAnnotateRouteSliderChange(type) {
     if (this.annotate_polyline === null) return;
     this.setState({ annotate_route_type: type }, () =>
-      this.annotate_polyline.setStyle({ color: ROUTE_TYPE_COLOR[type % ROUTE_TYPE_COLOR.length] })
+      this.annotate_polyline.setStyle({ 
+        color: ANNOTATE_LINE_COLOR,
+        opacity: GRAPH_OPACITY,
+        weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
+        pane: "graph",
+        lineCap: "butt",
+      })
     );
   }
 
@@ -1243,7 +1343,7 @@ class GraphMap extends React.Component {
       let new_diff_rot_p = new_rot_loc_p.subtract(trans_loc_p);
       let rot_mag = Math.sqrt(Math.pow(diff_rot_p.x, 2) + Math.pow(diff_rot_p.y, 2));
       let new_rot_mag = Math.sqrt(Math.pow(new_diff_rot_p.x, 2) + Math.pow(new_diff_rot_p.y, 2));
-      if (new_rot_mag == 0) return;
+      if (new_rot_mag === 0) return;
       new_rot_loc_p = trans_loc_p.add(new_diff_rot_p.multiplyBy(rot_mag / new_rot_mag));
 
       // Scale marker moves with rot marker
@@ -1280,7 +1380,7 @@ class GraphMap extends React.Component {
       let new_diff_scale_p = new_scale_loc_p.subtract(trans_loc_p);
       let scale_mag = Math.sqrt(Math.pow(diff_scale_p.x, 2) + Math.pow(diff_scale_p.y, 2));
       let new_scale_mag = Math.sqrt(Math.pow(new_diff_scale_p.x, 2) + Math.pow(new_diff_scale_p.y, 2));
-      if (new_scale_mag == 0) return;
+      if (new_scale_mag === 0) return;
       new_scale_loc_p = trans_loc_p.add(diff_scale_p.multiplyBy(new_scale_mag / scale_mag));
 
       // Scale marker moves with rot marker
@@ -1333,6 +1433,14 @@ class GraphMap extends React.Component {
       this.map.getPane("graph").style.zIndex = -100;
     };
     this.moveGraphZoomEnd = () => {
+      this.fixed_routes.forEach((route) => {
+        route.polyline = this.route2Polyline(route);
+      });
+      this.active_routes.forEach((route) => {
+        route.polyline = this.route2Polyline(route);
+      });
+      // display markers
+      this.scale_marker.setOpacity(MOVE_GRAPH_MARKER_OPACITY2);
       // Maintain the relative position of rotMarker and transMarker
       let trans_loc_p = this.map.latLngToLayerPoint(trans_loc);
       let new_rot_loc_p = trans_loc_p.add(diff_rot_p_zoom);
@@ -1422,6 +1530,14 @@ class GraphMap extends React.Component {
     // add the robot marker back
     this.robot_marker.marker.addTo(this.map);
   }
+
+  metres2pix(metres) {
+    let origin = L.latLng(this.id2vertex.get(this.root_vid));
+    let origin_plus_metre = this.map.latLngToLayerPoint(L.latLng(origin.lat - 1, origin.lng));
+    let metre_standard = origin_plus_metre.subtract(this.map.latLngToLayerPoint(origin));
+    let factor = metre_standard.y * 0.000009;
+    return metres * factor;
+  }
 }
 
-export default GraphMap;
+export default GraphMap;
\ No newline at end of file
diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/setup/StartMenu.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/setup/StartMenu.js
index 434bba4b..c564b7d8 100644
--- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/setup/StartMenu.js
+++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/setup/StartMenu.js
@@ -92,6 +92,7 @@ class StartMenu extends React.Component {constructor(props) {
                 width: "90%",
                 margin: "5% 10% 0% 0%",
               }}
+              alt="ASRL"
             />
             <div
               style={{
diff --git a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
index 837ceeb8..1e08002c 100644
--- a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
+++ b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
@@ -18,7 +18,10 @@
  */
 #pragma once
 
+#include <utility>
 #include <proj.h>
+#include <math.h>
+#include <cmath>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -28,6 +31,7 @@
 
 #include "vtr_navigation_msgs/msg/annotate_route.hpp"
 #include "vtr_navigation_msgs/msg/update_waypoint.hpp"
+#include "vtr_navigation_msgs/msg/mission_command.hpp"
 #include "vtr_navigation_msgs/msg/graph_route.hpp"
 #include "vtr_navigation_msgs/msg/graph_state.hpp"
 #include "vtr_navigation_msgs/msg/graph_update.hpp"
@@ -38,6 +42,7 @@
 #include "vtr_navigation_msgs/srv/robot_state.hpp"
 #include "vtr_pose_graph_msgs/msg/map_info.hpp"
 #include "vtr_pose_graph_msgs/srv/map_info.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
 
 namespace vtr {
 namespace navigation {
@@ -65,6 +70,8 @@ class GraphMapServer : public tactic::Graph::Callback,
   using MoveGraphMsg = vtr_navigation_msgs::msg::MoveGraph;
   using AnnotateRouteMsg = vtr_navigation_msgs::msg::AnnotateRoute;
   using UpdateWaypointMsg = vtr_navigation_msgs::msg::UpdateWaypoint;
+  using MissionCommandMsg = vtr_navigation_msgs::msg::MissionCommand;
+  using GoalHandle = vtr_navigation_msgs::msg::GoalHandle;
 
   using VertexPtr = tactic::Graph::VertexPtr;
   using EdgePtr = tactic::Graph::EdgePtr;
@@ -76,6 +83,8 @@ class GraphMapServer : public tactic::Graph::Callback,
   using GraphWeakPtr = tactic::Graph::WeakPtr;
   using GraphBasePtr = tactic::GraphBase::Ptr;
 
+  using NavSatFix = sensor_msgs::msg::NavSatFix;
+
   using VertexId2TransformMap = std::unordered_map<VertexId, Transform>;
   using VertexId2IdxMap = std::unordered_map<VertexId, size_t>;
   using ProjectVertex =
@@ -105,7 +114,9 @@ class GraphMapServer : public tactic::Graph::Callback,
       std::shared_ptr<MapInfoSrv::Response> response) const;
   void moveGraphCallback(const MoveGraphMsg::ConstSharedPtr msg);
   void annotateRouteCallback(const AnnotateRouteMsg::ConstSharedPtr msg);
+  void poseCallback(const NavSatFix::ConstSharedPtr msg);
   void updateWaypointCallback(const UpdateWaypointMsg::ConstSharedPtr msg);
+  void updateMissionCallback(const MissionCommandMsg::ConstSharedPtr msg);
 
  private:
   /// these functions are called with graph mutex locked
@@ -135,6 +146,10 @@ class GraphMapServer : public tactic::Graph::Callback,
 
   void updateRobotProjection();
 
+  float haversineDist(float lat1, float lat2, float lon1, float lon2);
+  float deltaLongToMetres(float lat1, float lat2, float lon1, float lon2);
+  float deltaLatToMetres(float lat1, float lat2);
+
  private:
   /** \brief Graph that generates the callbacks */
   GraphWeakPtr graph_;
@@ -149,6 +164,8 @@ class GraphMapServer : public tactic::Graph::Callback,
   /** \brief Vertices and routes */
   GraphState graph_state_;
 
+  uint goal_;
+
   /**
    * \brief Cached robot persistent & target localization used after graph
    * relaxation and calibration
@@ -189,6 +206,13 @@ class GraphMapServer : public tactic::Graph::Callback,
   rclcpp::Subscription<MoveGraphMsg>::SharedPtr move_graph_sub_;
   rclcpp::Subscription<AnnotateRouteMsg>::SharedPtr annotate_route_sub_;
   rclcpp::Subscription<UpdateWaypointMsg>::SharedPtr update_waypoint_sub_;
+  rclcpp::Subscription<MissionCommandMsg>::SharedPtr mission_command_sub_;
+
+  rclcpp::Subscription<NavSatFix>::SharedPtr pose_pub_;
+
+  const float dist_thres_ = 0.1;
+  bool initial_pose_set_ = false;
+  std::deque<std::pair<int, int>> gps_coords_ {2};
 };
 
 class RvizGraphMapServer : public GraphMapServer,
diff --git a/main/src/vtr_navigation/src/graph_map_server.cpp b/main/src/vtr_navigation/src/graph_map_server.cpp
index 8ef88fd7..871eb70d 100644
--- a/main/src/vtr_navigation/src/graph_map_server.cpp
+++ b/main/src/vtr_navigation/src/graph_map_server.cpp
@@ -66,6 +66,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
   const auto lng = node->declare_parameter<double>("graph_projection.origin_lng", -79.466092);
   const auto theta = node->declare_parameter<double>("graph_projection.origin_theta", 0.);
   const auto scale = node->declare_parameter<double>("graph_projection.scale", 1.);
+  const auto pose_pub_topic_ = node->declare_parameter<std::string>("graph_projection.gps_topic", "/novatel/fix");
 
   /// Publishers and services
   callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -80,14 +81,18 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
   following_route_pub_ = node->create_publisher<FollowingRoute>("following_route", 10);
   following_route_srv_ = node->create_service<FollowingRouteSrv>("following_route_srv", std::bind(&GraphMapServer::followingRouteSrvCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_);
 
+
    // graph manipulation
   auto sub_opt = rclcpp::SubscriptionOptions();
   sub_opt.callback_group = callback_group_;
   annotate_route_sub_ = node->create_subscription<AnnotateRouteMsg>("annotate_route", rclcpp::QoS(10), std::bind(&GraphMapServer::annotateRouteCallback, this, std::placeholders::_1), sub_opt);
   move_graph_sub_ = node->create_subscription<MoveGraphMsg>("move_graph", rclcpp::QoS(10), std::bind(&GraphMapServer::moveGraphCallback, this, std::placeholders::_1), sub_opt);
   update_waypoint_sub_ = node->create_subscription<UpdateWaypointMsg>("update_waypoint", rclcpp::QoS(10), std::bind(&GraphMapServer::updateWaypointCallback, this, std::placeholders::_1), sub_opt);
+  mission_command_sub_ = node->create_subscription<MissionCommandMsg>("mission_command", rclcpp::QoS(10), std::bind(&GraphMapServer::updateMissionCallback, this, std::placeholders::_1), sub_opt);
   // clang-format on
 
+  pose_pub_ = node->create_subscription<NavSatFix>(pose_pub_topic_, rclcpp::QoS(10), std::bind(&GraphMapServer::poseCallback, this, std::placeholders::_1), sub_opt);
+
   // initialize graph mapinfo if working on a new map
   auto map_info = graph->getMapInfo();
   if (!map_info.set) {
@@ -151,6 +156,11 @@ void GraphMapServer::followingRouteSrvCallback(
   response->following_route = following_route_;
 }
 
+void GraphMapServer::updateMissionCallback(
+    const MissionCommandMsg::ConstSharedPtr msg) {
+  goal_ = msg->goal_handle.type;
+}
+
 void GraphMapServer::annotateRouteCallback(
     const AnnotateRouteMsg::ConstSharedPtr msg) {
   CLOG(DEBUG, "navigation.graph_map_server")
@@ -206,6 +216,52 @@ void GraphMapServer::moveGraphCallback(const MoveGraphMsg::ConstSharedPtr msg) {
   graph_state_pub_->publish(graph_state_);
 }
 
+float GraphMapServer::haversineDist(float lat1, float lat2, float lon1, float lon2) {
+  float R = 6378137.0; // Radius of earth in metres
+  float dLat = lat2 * M_PI / 180 - lat1 * M_PI / 180;
+  float dLon = lon2 * M_PI / 180 - lon1 * M_PI / 180;
+  float a = std::sin(dLat/2) * std::sin(dLat/2) +
+    std::cos(lat1 * M_PI / 180) * std::cos(lat2 * M_PI / 180) *
+    std::sin(dLon/2) * std::sin(dLon/2);
+  float c = 2 * atan2(std::sqrt(a), std::sqrt(1-a));
+  float d = R * c;
+  return d;
+}
+
+float GraphMapServer::deltaLongToMetres(float lat1, float lat2, float lon1, float lon2) {
+  float avg_lat = (lat1 + lat2) / 2;
+  float dist = (lon2 - lon1) * std::cos(avg_lat * M_PI / 180) * 111000;
+  return dist;
+}
+
+float GraphMapServer::deltaLatToMetres(float lat1, float lat2) {
+  float dist = (lat2 - lat1) * 111000;
+  return dist;
+}
+
+void GraphMapServer::poseCallback(const NavSatFix::ConstSharedPtr msg) {
+  gps_coords_.push_back(std::make_pair(msg->longitude, msg->latitude));
+  gps_coords_.pop_front();
+  const auto graph = getGraph();
+  auto map_info = graph->getMapInfo();
+
+  auto prev_coords = gps_coords_[0];
+  auto dist = haversineDist(prev_coords.second, msg->latitude, prev_coords.first, msg->longitude);
+
+  if (goal_ == GoalHandle::TEACH && gps_coords_.size() >= 2 && !initial_pose_set_ && dist > dist_thres_) {
+    auto delta_lng = deltaLongToMetres(prev_coords.second, msg->latitude, prev_coords.first, msg->longitude);
+    auto delta_lat = deltaLatToMetres(prev_coords.second, msg->latitude);
+
+    map_info.root_vid = 0;
+    map_info.lng = (float) msg->longitude;
+    map_info.lat = (float) msg->latitude;
+    map_info.theta = (float) atan2(delta_lat, delta_lng);
+    map_info.set = true;
+    graph->setMapInfo(map_info);
+    initial_pose_set_ = true;
+  }
+}
+
 void GraphMapServer::updateWaypointCallback(
     const UpdateWaypointMsg::ConstSharedPtr msg) {
   CLOG(DEBUG, "navigation.graph_map_server")