diff --git a/config/wato_asd_training_foxglove_config .json b/config/wato_asd_training_foxglove_config .json
index 2558ee1f..d986a233 100644
--- a/config/wato_asd_training_foxglove_config .json	
+++ b/config/wato_asd_training_foxglove_config .json	
@@ -129,7 +129,7 @@
           "instanceId": "9e830917-d6fe-4f25-8e7b-25afa5a47d21",
           "layerId": "foxglove.Urdf",
           "sourceType": "url",
-          "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/development/src/gazebo/launch/robot.urdf",
+          "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/main/src/gazebo/launch/robot.urdf",
           "filePath": "",
           "parameter": "",
           "topic": "",
@@ -167,7 +167,7 @@
           "instanceId": "cadf14d8-072e-4af6-8fd0-60bf81d00399",
           "layerId": "foxglove.Urdf",
           "sourceType": "url",
-          "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/reference/src/gazebo/launch/env.urdf",
+          "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/main/src/gazebo/launch/env.urdf",
           "filePath": "",
           "parameter": "",
           "topic": "",
diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp
index 7a63619c..85bd9673 100644
--- a/src/robot/control/include/control_node.hpp
+++ b/src/robot/control/include/control_node.hpp
@@ -5,58 +5,31 @@
 #include "nav_msgs/msg/path.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/twist.hpp"
-
-#include "control_core.hpp"
+#include <vector>
+#include <cmath>
 
 class ControlNode : public rclcpp::Node {
-  public:
+public:
     ControlNode();
 
-    // Read and load in ROS2 parameters
-    void processParameters();
-
-    // Utility: Convert quaternion to yaw
-    double quaternionToYaw(double x, double y, double z, double w);
-
-    // Callback for path
-    void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
-
-    // Callback for odometry
-    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
-
-    // Main loop to continuously follow the path
-    void followPath();
+private:
+    void pathCallback(const nav_msgs::msg::Path::SharedPtr path_msg);
+    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg);
+    void computeControlCommands();
 
-    // Timer callback
-    void timerCallback();
+    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_;
 
-  private:
-    robot::ControlCore control_;
+    nav_msgs::msg::Path current_path_;
+    geometry_msgs::msg::Pose current_pose_;
+    bool path_received_;
+    bool odom_received_;
 
-    // Subscriber and Publisher
-    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_subscriber_;
-    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
-
-    // Timer
-    rclcpp::TimerBase::SharedPtr timer_;
-
-    // Path and robot state
-    double robot_x_;
-    double robot_y_;
-    double robot_theta_;
-
-    // ROS2 params
-    std::string path_topic_;
-    std::string odom_topic_;
-    std::string cmd_vel_topic_;
-    
-    int control_period_ms_;
+    // Control parameters
     double lookahead_distance_;
-    double steering_gain_;
-
-    double max_steering_angle_;
-    double linear_velocity_;
+    double max_linear_speed_;
+    double max_angular_speed_;
 };
 
 #endif
diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp
index a10a9662..15b459f0 100644
--- a/src/robot/control/src/control_node.cpp
+++ b/src/robot/control/src/control_node.cpp
@@ -1,101 +1,89 @@
-#include "tf2/LinearMath/Quaternion.h"
-#include "tf2/LinearMath/Matrix3x3.h"
-
 #include "control_node.hpp"
 
-ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger()))
-{
-  processParameters();
-
-  // Initialize subscribers and publisher
-  path_subscriber_ = this->create_subscription<nav_msgs::msg::Path>(
-    path_topic_, 10, std::bind(&ControlNode::pathCallback, this, std::placeholders::_1));
-
-  odom_subscriber_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    odom_topic_, 10, std::bind(&ControlNode::odomCallback, this, std::placeholders::_1));
-
-  cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(cmd_vel_topic_, 10);
-
-  timer_ = this->create_wall_timer(
-    std::chrono::milliseconds(control_period_ms_),
-    std::bind(&ControlNode::timerCallback, this)
-  );
+ControlNode::ControlNode()
+    : Node("control_node"), path_received_(false), odom_received_(false),
+      lookahead_distance_(1.0), max_linear_speed_(0.5), max_angular_speed_(1.0) {
+    
+    path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+        "/path", 10, std::bind(&ControlNode::pathCallback, this, std::placeholders::_1));
 
-  control_.initControlCore(lookahead_distance_, max_steering_angle_, steering_gain_, linear_velocity_);
-}
+    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+        "/odom/filtered", 10, std::bind(&ControlNode::odomCallback, this, std::placeholders::_1));
 
-void ControlNode::processParameters() {
-  // Declare all ROS2 Parameters
-  this->declare_parameter<std::string>("path_topic", "/path");
-  this->declare_parameter<std::string>("odom_topic", "/odom/filtered");
-  this->declare_parameter<std::string>("cmd_vel_topic", "/cmd_vel");
-  this->declare_parameter<int>("control_period_ms", 100);
-  this->declare_parameter<double>("lookahead_distance", 1.0);
-  this->declare_parameter<double>("steering_gain", 1.5);
-  this->declare_parameter<double>("max_steering_angle", 1.5);
-  this->declare_parameter<double>("linear_velocity", 1.5);
-
-  // Retrieve parameters and store them in member variables
-  path_topic_ = this->get_parameter("path_topic").as_string();
-  odom_topic_ = this->get_parameter("odom_topic").as_string();
-  cmd_vel_topic_ = this->get_parameter("cmd_vel_topic").as_string();
-  control_period_ms_ = this->get_parameter("control_period_ms").as_int();
-  lookahead_distance_ = this->get_parameter("lookahead_distance").as_double();
-  steering_gain_ = this->get_parameter("steering_gain").as_double();
-  max_steering_angle_ = this->get_parameter("max_steering_angle").as_double();
-  linear_velocity_ = this->get_parameter("linear_velocity").as_double();
-}
+    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
 
-void ControlNode::pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
-{
-  control_.updatePath(*msg);
+    RCLCPP_INFO(this->get_logger(), "Control Node initialized");
 }
 
-void ControlNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
-{
-  robot_x_ = msg->pose.pose.position.x;
-  robot_y_ = msg->pose.pose.position.y;
-
-  // Get robot's orientation (yaw) from quaternion using utility function
-  robot_theta_ = quaternionToYaw(
-    msg->pose.pose.orientation.x,
-    msg->pose.pose.orientation.y,
-    msg->pose.pose.orientation.z,
-    msg->pose.pose.orientation.w
-  );
-}
-
-void ControlNode::followPath()
-{
-  if (control_.isPathEmpty())
-  {
-    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "Path is empty. Waiting for new path.");
-  }
-
-  // Calculate control commands
-  geometry_msgs::msg::Twist cmd_vel = control_.calculateControlCommand(robot_x_, robot_y_, robot_theta_);
-  cmd_vel_publisher_->publish(cmd_vel);
+void ControlNode::pathCallback(const nav_msgs::msg::Path::SharedPtr path_msg) {
+    current_path_ = *path_msg;
+    path_received_ = true;
+    computeControlCommands();
 }
 
-void ControlNode::timerCallback()
-{
-  followPath();
+void ControlNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) {
+    current_pose_ = odom_msg->pose.pose;
+    odom_received_ = true;
+    if (path_received_) {
+        computeControlCommands();
+    }
 }
 
-double ControlNode::quaternionToYaw(double x, double y, double z, double w)
-{
-    // Using tf2 to convert to RPY
-    tf2::Quaternion q(x, y, z, w);
-    tf2::Matrix3x3 m(q);
-    double roll, pitch, yaw;
-    m.getRPY(roll, pitch, yaw);
-    return yaw;
+void ControlNode::computeControlCommands() {
+    if (current_path_.poses.empty()) {
+        RCLCPP_WARN(this->get_logger(), "Path is empty, stopping the robot.");
+        geometry_msgs::msg::Twist stop_msg;
+        cmd_vel_pub_->publish(stop_msg);
+        return;
+    }
+
+    // Find the closest waypoint within the lookahead distance
+    geometry_msgs::msg::PoseStamped target_pose;
+    bool target_found = false;
+
+    for (const auto &pose : current_path_.poses) {
+        double dx = pose.pose.position.x - current_pose_.position.x;
+        double dy = pose.pose.position.y - current_pose_.position.y;
+        double distance = std::sqrt(dx * dx + dy * dy);
+
+        if (distance > lookahead_distance_) {
+            target_pose = pose;
+            target_found = true;
+            break;
+        }
+    }
+
+    if (!target_found) {
+        RCLCPP_WARN(this->get_logger(), "No target waypoint found within lookahead distance.");
+        return;
+    }
+
+    // Compute control commands using Pure Pursuit
+    double dx = target_pose.pose.position.x - current_pose_.position.x;
+    double dy = target_pose.pose.position.y - current_pose_.position.y;
+
+    double yaw = std::atan2(2.0 * (current_pose_.orientation.w * current_pose_.orientation.z),
+                            1.0 - 2.0 * (current_pose_.orientation.z * current_pose_.orientation.z));
+
+    double target_angle = std::atan2(dy, dx);
+    double angle_diff = target_angle - yaw;
+
+    // Normalize angle to [-pi, pi]
+    while (angle_diff > M_PI) angle_diff -= 2.0 * M_PI;
+    while (angle_diff < -M_PI) angle_diff += 2.0 * M_PI;
+
+    geometry_msgs::msg::Twist cmd_vel;
+    cmd_vel.linear.x = std::min(max_linear_speed_, std::sqrt(dx * dx + dy * dy));
+    cmd_vel.angular.z = std::min(max_angular_speed_, angle_diff);
+
+    cmd_vel_pub_->publish(cmd_vel);
+    RCLCPP_INFO(this->get_logger(), "Published velocity command: linear=%.2f, angular=%.2f",
+                cmd_vel.linear.x, cmd_vel.angular.z);
 }
 
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<ControlNode>());
-  rclcpp::shutdown();
-  return 0;
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<ControlNode>());
+    rclcpp::shutdown();
+    return 0;
 }
diff --git a/src/robot/costmap/include/costmap_node.hpp b/src/robot/costmap/include/costmap_node.hpp
index d9dc17d2..9d18c304 100644
--- a/src/robot/costmap/include/costmap_node.hpp
+++ b/src/robot/costmap/include/costmap_node.hpp
@@ -4,36 +4,31 @@
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
-
-#include "costmap_core.hpp"
+#include <vector>
+#include <cmath>
 
 class CostmapNode : public rclcpp::Node {
-  public:
-    // Costmap Node constructor
+public:
     CostmapNode();
-    
-    // Retrieves all the parameters and their values in params.yaml
-    void processParameters();
-    
-    // Given a laserscan, it will send it into CostmapCore for processing and then
-    // retrieve the costmap
-    void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) const;
 
-  private:
-    robot::CostmapCore costmap_;
+private:
+    void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
+    void initializeCostmap();
+    void convertToGrid(double range, double angle, int &x_grid, int &y_grid);
+    void markObstacle(int x_grid, int y_grid);
+    void inflateObstacles();
+    void publishCostmap();
 
-    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
     rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
 
-    std::string laserscan_topic_;
-    std::string costmap_topic_;
-
-    double resolution_;
+    std::vector<int8_t> costmap_;
     int width_;
     int height_;
-    geometry_msgs::msg::Pose origin_;
+    double resolution_;
+    double origin_x_;
+    double origin_y_;
     double inflation_radius_;
-
 };
 
-#endif 
\ No newline at end of file
+#endif
diff --git a/src/robot/costmap/src/costmap_node.cpp b/src/robot/costmap/src/costmap_node.cpp
index 85dbe4d5..c25f43b0 100644
--- a/src/robot/costmap/src/costmap_node.cpp
+++ b/src/robot/costmap/src/costmap_node.cpp
@@ -1,71 +1,104 @@
-#include <chrono>
-#include <memory>
-
 #include "costmap_node.hpp"
 
-CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) {
-  // load ROS2 yaml parameters
-  processParameters();
+CostmapNode::CostmapNode() 
+    : Node("costmap_node"), width_(200), height_(200), resolution_(0.1),
+      origin_x_(-10.0), origin_y_(-10.0), inflation_radius_(1.0) {
+    
+    laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+        "/lidar", 10, std::bind(&CostmapNode::laserCallback, this, std::placeholders::_1));
+    costmap_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("/costmap", 10);
+
+    costmap_.resize(width_ * height_, 0);
+    RCLCPP_INFO(this->get_logger(), "Costmap Node initialized");
+}
 
-  laser_scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
-    laserscan_topic_, 10, 
-    std::bind(
-      &CostmapNode::laserScanCallback, this, 
-      std::placeholders::_1));
+void CostmapNode::initializeCostmap() {
+    std::fill(costmap_.begin(), costmap_.end(), 0);  // Reset the costmap to free space
+}
+
+void CostmapNode::convertToGrid(double range, double angle, int &x_grid, int &y_grid) {
+    double x_world = range * cos(angle);
+    double y_world = range * sin(angle);
+
+    x_grid = static_cast<int>((x_world - origin_x_) / resolution_);
+    y_grid = static_cast<int>((y_world - origin_y_) / resolution_);
+}
 
-  costmap_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
-    costmap_topic_, 10);
+void CostmapNode::markObstacle(int x_grid, int y_grid) {
+    if (x_grid >= 0 && x_grid < width_ && y_grid >= 0 && y_grid < height_) {
+        int index = y_grid * width_ + x_grid;
+        costmap_[index] = 100;  // Mark as an obstacle
+    }
+}
 
-  RCLCPP_INFO(this->get_logger(), "Initialized ROS Constructs");
+void CostmapNode::inflateObstacles() {
+    std::vector<int8_t> inflated_costmap = costmap_;  // Copy the current costmap
 
-  costmap_.initCostmap(
-    resolution_, 
-    width_, 
-    height_, 
-    origin_,
-    inflation_radius_
-  );
+    int cells_inflation_radius = static_cast<int>(inflation_radius_ / resolution_);
+    for (int y = 0; y < height_; ++y) {
+        for (int x = 0; x < width_; ++x) {
+            int index = y * width_ + x;
+            if (costmap_[index] == 100) {  // If this cell is an obstacle
+                for (int dy = -cells_inflation_radius; dy <= cells_inflation_radius; ++dy) {
+                    for (int dx = -cells_inflation_radius; dx <= cells_inflation_radius; ++dx) {
+                        int nx = x + dx;
+                        int ny = y + dy;
+                        if (nx >= 0 && nx < width_ && ny >= 0 && ny < height_) {
+                            double distance = std::sqrt(dx * dx + dy * dy) * resolution_;
+                            if (distance <= inflation_radius_) {
+                                int neighbor_index = ny * width_ + nx;
+                                inflated_costmap[neighbor_index] = static_cast<int8_t>(
+                                    std::max(static_cast<int>(inflated_costmap[neighbor_index]), 
+                                             static_cast<int>(100 * (1 - distance / inflation_radius_)))
+                                );
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
 
-  RCLCPP_INFO(this->get_logger(), "Initialized Costmap Core");
+    costmap_ = inflated_costmap;  // Update the costmap
 }
 
-void CostmapNode::processParameters() {
-  // Declare all ROS2 Parameters
-  this->declare_parameter<std::string>("laserscan_topic", "/lidar");
-  this->declare_parameter<std::string>("costmap_topic", "/costmap");
-  this->declare_parameter<double>("costmap.resolution", 0.1);
-  this->declare_parameter<int>("costmap.width", 100);
-  this->declare_parameter<int>("costmap.height", 100);
-  this->declare_parameter<double>("costmap.origin.position.x", -5.0);
-  this->declare_parameter<double>("costmap.origin.position.y", -5.0);
-  this->declare_parameter<double>("costmap.origin.orientation.w", 1.0);
-  this->declare_parameter<double>("costmap.inflation_radius", 1.0);
-
-  // Retrieve parameters and store them in member variables
-  laserscan_topic_ = this->get_parameter("laserscan_topic").as_string();
-  costmap_topic_ = this->get_parameter("costmap_topic").as_string();
-  resolution_ = this->get_parameter("costmap.resolution").as_double();
-  width_ = this->get_parameter("costmap.width").as_int();
-  height_ = this->get_parameter("costmap.height").as_int();
-  origin_.position.x = this->get_parameter("costmap.origin.position.x").as_double();
-  origin_.position.y = this->get_parameter("costmap.origin.position.y").as_double();
-  origin_.orientation.w = this->get_parameter("costmap.origin.orientation.w").as_double();
-  inflation_radius_ = this->get_parameter("costmap.inflation_radius").as_double();
+void CostmapNode::publishCostmap() {
+    nav_msgs::msg::OccupancyGrid costmap_msg;
+    costmap_msg.header.stamp = this->now();
+    costmap_msg.header.frame_id = "map";
+
+    costmap_msg.info.resolution = resolution_;
+    costmap_msg.info.width = width_;
+    costmap_msg.info.height = height_;
+    costmap_msg.info.origin.position.x = origin_x_;
+    costmap_msg.info.origin.position.y = origin_y_;
+    costmap_msg.info.origin.position.z = 0.0;
+
+    costmap_msg.data = costmap_;
+
+    costmap_pub_->publish(costmap_msg);
 }
 
-void CostmapNode::laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) const {
-  // Update the costmap according to the laser scan
-  costmap_.updateCostmap(msg);
-  // publish the costmap
-  nav_msgs::msg::OccupancyGrid costmap_msg = *costmap_.getCostmapData();
-  costmap_msg.header = msg->header;
-  costmap_pub_->publish(costmap_msg);
+void CostmapNode::laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan) {
+    initializeCostmap();
+
+    for (size_t i = 0; i < scan->ranges.size(); ++i) {
+        double angle = scan->angle_min + i * scan->angle_increment;
+        double range = scan->ranges[i];
+        if (range > scan->range_min && range < scan->range_max) {
+            int x_grid, y_grid;
+            convertToGrid(range, angle, x_grid, y_grid);
+            markObstacle(x_grid, y_grid);
+        }
+    }
+
+    inflateObstacles();
+    publishCostmap();
 }
 
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<CostmapNode>());
-  rclcpp::shutdown();
-  return 0;
-}
\ No newline at end of file
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<CostmapNode>());
+    rclcpp::shutdown();
+    return 0;
+}
diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner_node.hpp
index 55c3c0a1..4c6dc792 100644
--- a/src/robot/planner/include/planner_node.hpp
+++ b/src/robot/planner/include/planner_node.hpp
@@ -2,30 +2,25 @@
 #define PLANNER_NODE_HPP_
 
 #include <mutex>
-
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
-
 #include "planner_core.hpp"
 
 class PlannerNode : public rclcpp::Node {
-  public:
+public:
     PlannerNode();
-
     void processParameters();
-
     void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map_msg);
-    void goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr  goal_msg);
+    void goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr goal_msg);
     void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg);
     void timerCallback();
-
     void publishPath();
     void resetGoal();
 
-  private:
+private:
     robot::PlannerCore planner_;
 
     // Subscriptions
@@ -48,10 +43,12 @@ class PlannerNode : public rclcpp::Node {
     bool active_goal_;
     rclcpp::Time plan_start_time_;
 
-    // Robot odometry (x,y). For simplicity, ignoring orientation usage here.
+    // Robot odometry
     bool have_odom_;
     double odom_x_;
     double odom_y_;
+    double planned_x_;
+    double planned_y_;
 
     // Parameters
     std::string map_topic_;
@@ -64,7 +61,6 @@ class PlannerNode : public rclcpp::Node {
 
     double goal_tolerance_;
     double plan_timeout_;
-
 };
 
 #endif 
diff --git a/src/robot/planner/src/planner_node.cpp b/src/robot/planner/src/planner_node.cpp
index 13a0729b..17d24f2e 100644
--- a/src/robot/planner/src/planner_node.cpp
+++ b/src/robot/planner/src/planner_node.cpp
@@ -1,190 +1,150 @@
 #include "planner_node.hpp"
 
 PlannerNode::PlannerNode() : Node("planner"), planner_(robot::PlannerCore(this->get_logger())) {
-  // load ROS2 yaml parameters
-  processParameters();
-
-  // Subscribers
-  map_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
-    map_topic_,
-    10,
-    std::bind(&PlannerNode::mapCallback, this, std::placeholders::_1)
-  );
-
-  goal_sub_ = this->create_subscription<geometry_msgs::msg::PointStamped>(
-    goal_topic_,
-    10,
-    std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1)
-  );
-
-  // Subscribe to odometry from /odom/filtered
-  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    odom_topic_,
-    10,
-    std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1)
-  );
-
-  // Publisher
-  path_pub_ = this->create_publisher<nav_msgs::msg::Path>(path_topic_, 10);
-
-  // Timer to check goal/timeout status periodically (500 ms)
-  timer_ = this->create_wall_timer(
-    std::chrono::milliseconds(500),
-    std::bind(&PlannerNode::timerCallback, this)
-  );
-
-  planner_.initPlanner(smoothing_factor_, iterations_);
+    processParameters();
+
+    map_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
+        map_topic_, 10, std::bind(&PlannerNode::mapCallback, this, std::placeholders::_1));
+
+    goal_sub_ = this->create_subscription<geometry_msgs::msg::PointStamped>(
+        goal_topic_, 10, std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1));
+
+    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+        odom_topic_, 10, std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1));
+
+    path_pub_ = this->create_publisher<nav_msgs::msg::Path>(path_topic_, 10);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(500), std::bind(&PlannerNode::timerCallback, this));
+
+    planner_.initPlanner(smoothing_factor_, iterations_);
 }
 
 void PlannerNode::processParameters() {
-  // Declare and get parameters
-  this->declare_parameter<std::string>("map_topic", "/map");
-  this->declare_parameter<std::string>("goal_topic", "/goal_pose");
-  this->declare_parameter<std::string>("odom_topic", "/odom/filtered");
-  this->declare_parameter<std::string>("path_topic", "/path");
-  this->declare_parameter<double>("smoothing_factor", 0.2);
-  this->declare_parameter<int>("iterations", 20);
-  this->declare_parameter<double>("goal_tolerance", 0.3);
-  this->declare_parameter<double>("plan_timeout_seconds", 10.0);
-
-  map_topic_ = this->get_parameter("map_topic").as_string();
-  goal_topic_ = this->get_parameter("goal_topic").as_string();
-  odom_topic_ = this->get_parameter("odom_topic").as_string();
-  path_topic_ = this->get_parameter("path_topic").as_string();
-  smoothing_factor_ = this->get_parameter("smoothing_factor").as_double();
-  iterations_ = this->get_parameter("iterations").as_int();
-  goal_tolerance_ = this->get_parameter("goal_tolerance").as_double();
-  plan_timeout_ = this->get_parameter("plan_timeout_seconds").as_double();
+    this->declare_parameter<std::string>("map_topic", "/map");
+    this->declare_parameter<std::string>("goal_topic", "/goal_pose");
+    this->declare_parameter<std::string>("odom_topic", "/odom/filtered");
+    this->declare_parameter<std::string>("path_topic", "/path");
+    this->declare_parameter<double>("smoothing_factor", 0.2);
+    this->declare_parameter<int>("iterations", 20);
+    this->declare_parameter<double>("goal_tolerance", 0.3);
+    this->declare_parameter<double>("plan_timeout_seconds", 10.0);
+
+    map_topic_ = this->get_parameter("map_topic").as_string();
+    goal_topic_ = this->get_parameter("goal_topic").as_string();
+    odom_topic_ = this->get_parameter("odom_topic").as_string();
+    path_topic_ = this->get_parameter("path_topic").as_string();
+    smoothing_factor_ = this->get_parameter("smoothing_factor").as_double();
+    iterations_ = this->get_parameter("iterations").as_int();
+    goal_tolerance_ = this->get_parameter("goal_tolerance").as_double();
+    plan_timeout_ = this->get_parameter("plan_timeout_seconds").as_double();
 }
 
-void PlannerNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
-{
-  {
-    std::lock_guard<std::mutex> lock(map_mutex_);
-    map_ = msg;
-  }
+void PlannerNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
+    {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        map_ = msg;
+    }
 
-  // If we have an active goal, re-run plan
-  if (active_goal_) {
-    double elapsed = (now() - plan_start_time_).seconds();
-    if (elapsed <= plan_timeout_) {
-      RCLCPP_INFO(this->get_logger(), 
-                  "Map updated => Replanning for current goal (time elapsed: %.2f).",
-                  elapsed);
-      publishPath();
+    if (active_goal_) {
+        double elapsed = (now() - plan_start_time_).seconds();
+        if (elapsed <= plan_timeout_) {
+            RCLCPP_INFO(this->get_logger(), "Replanning for goal (elapsed: %.2f sec).", elapsed);
+            publishPath();
+        }
     }
-  }
 }
 
-void PlannerNode::goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr goal_msg)
-{
-  if (active_goal_) {
-    RCLCPP_WARN(this->get_logger(), "Ignoring new goal; a goal is already active.");
-    return;
-  }
-
-  if (!map_) {
-    RCLCPP_WARN(this->get_logger(), "No costmap available yet. Cannot set goal.");
-    return;
-  }
-
-  current_goal_ = *goal_msg;
-  active_goal_ = true;
-  plan_start_time_ = now();
+void PlannerNode::goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr goal_msg) {
+    if (active_goal_) {
+        RCLCPP_WARN(this->get_logger(), "New goal received. Replacing current goal.");
+        resetGoal();
+    }
 
-  RCLCPP_INFO(this->get_logger(), "Received new goal: (%.2f, %.2f)",
-              goal_msg->point.x, goal_msg->point.y);
+    current_goal_ = *goal_msg;
+    active_goal_ = true;
+    plan_start_time_ = now();
 
-  publishPath();
+    RCLCPP_INFO(this->get_logger(), "Accepted new goal: (%.2f, %.2f)", goal_msg->point.x, goal_msg->point.y);
+    publishPath();
 }
 
-void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg)
-{
-  // Store the latest odometry in (odom_x_, odom_y_)
-  // For simplicity, ignoring orientation or z
-  odom_x_ = odom_msg->pose.pose.position.x;
-  odom_y_ = odom_msg->pose.pose.position.y;
-  have_odom_ = true;
+void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) {
+    odom_x_ = odom_msg->pose.pose.position.x;
+    odom_y_ = odom_msg->pose.pose.position.y;
+    have_odom_ = true;
 }
 
-void PlannerNode::timerCallback()
-{
-  if (!active_goal_) {
-    return;
-  }
-
-  // Check if we've timed out
-  double elapsed = (now() - plan_start_time_).seconds();
-  if (elapsed > plan_timeout_) {
-    RCLCPP_WARN(this->get_logger(), "Plan timed out after %.2f seconds. Resetting goal.", elapsed);
-    resetGoal();
-    return;
-  }
-
-  // Check if we reached the goal
-  double distance = sqrt(pow(odom_x_ - current_goal_.point.x, 2) +  pow(odom_y_ - current_goal_.point.y, 2));
-  if (distance < goal_tolerance_) {
-    RCLCPP_WARN(this->get_logger(), "Plan succeeded! Elapsed Time: %.2f", elapsed);
-    resetGoal();
-    return;
-  }
+void PlannerNode::timerCallback() {
+    if (!active_goal_) {
+        return;
+    }
+
+    double elapsed = (now() - plan_start_time_).seconds();
+    double distance_to_goal = sqrt(pow(odom_x_ - current_goal_.point.x, 2) + pow(odom_y_ - current_goal_.point.y, 2));
+
+    if (elapsed > plan_timeout_) {
+        RCLCPP_WARN(this->get_logger(), "Plan timed out. Resetting goal.");
+        resetGoal();
+        return;
+    }
+
+    if (distance_to_goal < goal_tolerance_) {
+        RCLCPP_INFO(this->get_logger(), "Goal reached!");
+        resetGoal();
+        return;
+    }
+
+    double deviation_threshold = 1.0;
+    double planned_distance = sqrt(pow(planned_x_ - odom_x_, 2) + pow(planned_y_ - odom_y_, 2));
+
+    if (planned_distance > deviation_threshold) {
+        RCLCPP_WARN(this->get_logger(), "Robot deviated. Replanning...");
+        publishPath();
+    }
 }
 
 void PlannerNode::publishPath() {
-  if (!have_odom_) {
-    RCLCPP_WARN(this->get_logger(), "No odometry received yet. Cannot plan.");
-    resetGoal();
-    return;
-  }
-
-  // Use the robot's odometry as the start pose
-  double start_world_x = odom_x_;
-  double start_world_y = odom_y_;
-
-  {
-    std::lock_guard<std::mutex> lock(map_mutex_);
-    if(!planner_.planPath(start_world_x, start_world_y, current_goal_.point.x, current_goal_.point.y, map_)) {
-      RCLCPP_ERROR(this->get_logger(), "Plan Failed.");
-      resetGoal();
-      return;
+    if (!have_odom_) {
+        RCLCPP_WARN(this->get_logger(), "No odometry received yet.");
+        resetGoal();
+        return;
     }
-  }
 
-  nav_msgs::msg::Path path_msg = *planner_.getPath();
-  path_msg.header.stamp = this->now();
-  path_msg.header.frame_id = map_->header.frame_id;
-  
-  path_pub_->publish(path_msg);
-}
+    double start_x = odom_x_;
+    double start_y = odom_y_;
 
-void PlannerNode::resetGoal() {
-  active_goal_ = false;
-  RCLCPP_INFO(this->get_logger(), "Resetting active goal.");
-
-  // Publish an empty path, which should tell the robot to stop or have no path
-  nav_msgs::msg::Path empty_path;
-  empty_path.header.stamp = this->now();
-
-  // Use the costmap frame if available; otherwise a default like "sim_world"
-  {
-    std::lock_guard<std::mutex> lock(map_mutex_);
-    if (map_) {
-      empty_path.header.frame_id = map_->header.frame_id;
-    } else {
-      empty_path.header.frame_id = "sim_world";
+    {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        if (!planner_.planPath(start_x, start_y, current_goal_.point.x, current_goal_.point.y, map_)) {
+            RCLCPP_ERROR(this->get_logger(), "Planning failed.");
+            resetGoal();
+            return;
+        }
     }
-  }
-  
-  // Publish the empty path
-  path_pub_->publish(empty_path);
 
-  RCLCPP_INFO(this->get_logger(), "Published empty path to stop the robot.");
+    nav_msgs::msg::Path path_msg = *planner_.getPath();
+    path_msg.header.stamp = this->now();
+    path_msg.header.frame_id = map_->header.frame_id;
+    path_pub_->publish(path_msg);
+
+    if (!path_msg.poses.empty()) {
+        planned_x_ = path_msg.poses[0].pose.position.x;
+        planned_y_ = path_msg.poses[0].pose.position.y;
+    }
+}
+
+void PlannerNode::resetGoal() {
+    active_goal_ = false;
+    nav_msgs::msg::Path empty_path;
+    empty_path.header.stamp = this->now();
+    path_pub_->publish(empty_path);
+    RCLCPP_INFO(this->get_logger(), "Published empty path to stop the robot.");
 }
 
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<PlannerNode>());
-  rclcpp::shutdown();
-  return 0;
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<PlannerNode>());
+    rclcpp::shutdown();
+    return 0;
 }
diff --git a/watod-config.sh b/watod-config.sh
index bc5da50a..dac70f69 100755
--- a/watod-config.sh
+++ b/watod-config.sh
@@ -16,6 +16,7 @@
 ##   - 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.
@@ -39,3 +40,4 @@
 ## Platform in which to build the docker images with. 
 ## Either arm64 (apple silicon, raspberry pi) or amd64 (most computers)
 # PLATFORM="amd64"
+PLATFORM="arm64"
\ No newline at end of file