diff --git a/MobileRobotNavigation/__init__.py b/MobileRobotNavigation/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/dynamics.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/dynamics.py
new file mode 100644
index 0000000000..c9f2b1b457
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/dynamics.py
@@ -0,0 +1,134 @@
+"""
+Robot dynamics model.
+
+Author: Sleiman Safaoui (@The-SS)
+
+"""
+import numpy as np
+from PathTracking.stanley_controller.stanley_controller import normalize_angle
+
+
+class StanleyState(object):
+    """
+    Class representing the state of a vehicle.
+    Edited copy of the PathTracking.stanley_controller.stanley_controller State
+
+    :param x: (float) x-coordinate
+    :param y: (float) y-coordinate
+    :param yaw: (float) yaw angle
+    :param v: (float) speed
+    """
+
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, wb=1.0, max_steer=np.radians(30.0), dt=0.1):
+        """Instantiate the object."""
+        super(StanleyState, self).__init__()
+        # state
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.v = v
+        # parameters
+        self.wb = wb
+        self.max_steer = max_steer
+        # time step
+        self.dt = dt
+
+    def update(self, acceleration, delta):
+        """
+        Update the state of the vehicle.
+
+        Stanley Control uses bicycle model.
+
+        :param acceleration: (float) Acceleration
+        :param delta: (float) Steering
+        """
+        delta = np.clip(delta, -self.max_steer, self.max_steer)
+
+        self.x += self.v * np.cos(self.yaw) * self.dt
+        self.y += self.v * np.sin(self.yaw) * self.dt
+        self.yaw += self.v / self.wb * np.tan(delta) * self.dt
+        self.yaw = normalize_angle(self.yaw)
+        self.v += acceleration * self.dt
+
+    def reset_state(self, x, y, yaw, v):
+        """
+        Forcefully change the state.
+
+        :param x: (float) x-position
+        :param y: (float) y-position
+        :param yaw: (float) steering angle
+        :param v: (float) forward velocity
+        """
+        self.x, self.y, self.yaw, self.v = x, y, yaw, v
+
+
+class StanleyCtrl:
+    """
+    Stanley Controller class. Combines the functions from
+    PathTracking.stanley_controller.stanley_controller
+    into one class
+    """
+    def __init__(self, state: StanleyState, Kp: float, k: float):
+        self.state = state
+        self.Kp = Kp
+        self.k = k
+
+    def calc_target_index(self, cx, cy):
+        """
+        Compute index in the trajectory list of the target.
+
+        :param cx: [float]
+        :param cy: [float]
+        :return: (int, float)
+        """
+        # Calc front axle position
+        fx = self.state.x + self.state.wb * np.cos(self.state.yaw)
+        fy = self.state.y + self.state.wb * np.sin(self.state.yaw)
+
+        # Search nearest point index
+        dx = [fx - icx for icx in cx]
+        dy = [fy - icy for icy in cy]
+        d = np.hypot(dx, dy)
+        target_idx = np.argmin(d)
+
+        # Project RMS error onto front axle vector
+        front_axle_vec = [-np.cos(self.state.yaw + np.pi / 2),
+                          -np.sin(self.state.yaw + np.pi / 2)]
+        error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec)
+
+        return target_idx, error_front_axle
+
+    def pid_control(self, target, current):
+        """
+        Proportional control for the speed.
+
+        :param target: (float)
+        :param current: (float)
+        :return: (float)
+        """
+        return self.Kp * (target - current)
+
+    def stanley_control(self, cx, cy, cyaw, last_target_idx):
+        """
+        Stanley steering control.
+
+        :param cx: ([float])
+        :param cy: ([float])
+        :param cyaw: ([float])
+        :param last_target_idx: (int)
+        :return: (float, int)
+        """
+        current_target_idx, error_front_axle = self.calc_target_index(cx, cy)
+
+        if last_target_idx >= current_target_idx:
+            current_target_idx = last_target_idx
+
+        # theta_e corrects the heading error
+        theta_e = normalize_angle(cyaw[current_target_idx] - self.state.yaw)
+        print(theta_e)
+        # theta_d corrects the cross track error
+        theta_d = np.arctan2(self.k * error_front_axle, self.state.v)
+        # Steering control
+        delta = theta_e + theta_d
+
+        return delta, current_target_idx
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/helpers.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/helpers.py
new file mode 100644
index 0000000000..9ea44c9527
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/helpers.py
@@ -0,0 +1,35 @@
+"""
+Helper functions for the mobile autonomy stack
+
+Author: Sleiman Safaoui (@The-SS)
+
+"""
+import numpy as np
+
+
+def at_goal(gx, gy, x, y, thresh):
+    """
+    checks if the robot is close enough to the goal
+    :param gx: (float) goal x-values
+    :param gy: (float) goal y-values
+    :param x: (float) robot x-position
+    :param y: (float) robot y-position
+    :param thresh: (float) threshold for robot being close to the goal
+    """
+    return np.linalg.norm([x-gx, y-gy]) < thresh
+
+
+def in_collision(ox, oy, x, y, rad):
+    """
+    checks if the robot is in collision with and of the obstacles.
+    assumes robot is a circle and that the obstacles list is dense enough
+    :param ox: ([float]) obstacle x-values
+    :param oy: ([float]) obstacle y-values
+    :param x: (float) robot x-position
+    :param y: (float) robot y-position
+    :param rad: (float) robot radius
+    """
+    for px, py in zip(ox, oy):
+        if np.linalg.norm([x-px, y-py]) < rad:
+            return True
+    return False
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/lidar_sensing.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/lidar_sensing.py
new file mode 100644
index 0000000000..ead0f29827
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/lidar_sensing.py
@@ -0,0 +1,89 @@
+"""
+Defines function used to simulate a lidar.
+Lidar is assumed to always be aligned with the world frame for simplicity.
+
+All units are in meters.
+
+Author: Sleiman Safaoui (@The-SS)
+"""
+import numpy as np
+eps = 1e-6
+
+
+def _ray_with_horizontal_line(ray_theta, ys, y):
+    if abs(ray_theta) < eps or abs(ray_theta - np.pi) < eps:
+        return np.inf
+    if np.pi < ray_theta < 2*np.pi and ys < y:
+        return np.inf
+    if 0 < ray_theta < np.pi and ys > y:
+        return np.inf
+    return (y-ys) / np.sin(ray_theta)
+
+
+def _ray_with_vertical_line(ray_theta, xs, x):
+    if abs(ray_theta - np.pi/2) < eps or abs(ray_theta - 3*np.pi/2) < eps:
+        return np.inf
+    if np.pi/2 < ray_theta < 3*np.pi/2 and xs < x:
+        return np.inf
+    if (3*np.pi/2 < ray_theta or ray_theta < np.pi/2) and xs > x:
+        return np.inf
+    return (x-xs) / np.cos(ray_theta)
+
+
+def _polar_to_cart(xs, ys, r, theta, env_bounds):
+    xmin, xmax, ymin, ymax = env_bounds
+    if r == np.inf:
+        if abs(theta) < eps:
+            return xmax, ys
+        if abs(theta - np.pi/2) < eps:
+            return xs, ymax
+        if abs(theta - np.pi) < eps:
+            return xmin, ys
+        if abs(theta - 3*np.pi/2) < eps:
+            return xs, ymin
+    return r * np.cos(theta) + xs, r * np.sin(theta) + ys
+
+
+def _ray_to_obstacle(xs, ys, ray_theta, env_bounds, ob_list):
+    r_list = [np.inf]
+    xmin, xmax, ymin, ymax = env_bounds
+    r_list.append(_ray_with_horizontal_line(ray_theta, ys, ymin))
+    r_list.append(_ray_with_horizontal_line(ray_theta, ys, ymax))
+    r_list.append(_ray_with_vertical_line(ray_theta, xs, xmin))
+    r_list.append(_ray_with_vertical_line(ray_theta, xs, xmax))
+    for [xmin, xmax, ymin, ymax] in ob_list:
+        r = _ray_with_horizontal_line(ray_theta, ys, ymin)
+        x, y = _polar_to_cart(xs, ys, r, ray_theta, env_bounds)
+        if x < xmin or x > xmax:
+            r = np.inf
+        r_list.append(r)
+        r = _ray_with_horizontal_line(ray_theta, ys, ymax)
+        x, y = _polar_to_cart(xs, ys, r, ray_theta, env_bounds)
+        if x < xmin or x > xmax:
+            r = np.inf
+        r_list.append(r)
+        r = _ray_with_vertical_line(ray_theta, xs, xmin)
+        x, y = _polar_to_cart(xs, ys, r, ray_theta, env_bounds)
+        if y < ymin or y > ymax:
+            r = np.inf
+        r_list.append(r)
+        r = _ray_with_vertical_line(ray_theta, xs, xmax)
+        x, y = _polar_to_cart(xs, ys, r, ray_theta, env_bounds)
+        if y < ymin or y > ymax:
+            r = np.inf
+        r_list.append(r)
+    r = min(r_list)
+    x, y = _polar_to_cart(xs, ys, r, ray_theta, env_bounds)
+    return r, x, y
+
+
+def lidar_sensing(xs, ys, env_bounds, ob_list, count):
+    ray_theta_list = np.linspace(np.radians(0), 2*np.pi, count, endpoint=False)
+    r_list = [0 for _ in ray_theta_list]
+    x_list = [0 for _ in ray_theta_list]
+    y_list = [0 for _ in ray_theta_list]
+    for i in range(len(r_list)):
+        r_list[i], x_list[i], y_list[i] = _ray_to_obstacle(xs, ys, ray_theta_list[i], env_bounds, ob_list)
+    return r_list, x_list, y_list
+
+
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/perception_planning_and_control.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/perception_planning_and_control.py
new file mode 100644
index 0000000000..192cc3104a
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/perception_planning_and_control.py
@@ -0,0 +1,127 @@
+"""
+Planning and control stack for a mobile robot using existing algorithms in PythonRobotics.
+Planning with A*.
+
+Author: Sleiman Safaoui (@The-SS)
+
+"""
+import numpy as np
+
+from settings import sim_params
+from plotting import plot_env
+from PathPlanning.AStar.a_star import AStarPlanner
+import time
+from dynamics import StanleyState, StanleyCtrl
+import matplotlib.pyplot as plt
+from PathPlanning.CubicSpline import cubic_spline_planner
+# from PathTracking.stanley_controller.stanley_controller import pid_control, stanley_control
+from helpers import in_collision, at_goal
+from lidar_sensing import lidar_sensing
+
+
+t = 0  # simulation time
+t_sim = 20  # [sec] simulation time
+at_goal_thresh = 0.3
+
+# load simulation parameters
+params = sim_params()
+x0, y0 = params['start']
+yaw0 = np.radians(90)
+v0 = 0
+wb, max_steer = params['wheelbase'], params['max_steer']
+plan_rate, ctrl_rate = params['planning_rate'], params['control_rate']
+target_speed = params['max_speed']
+rob_rad = params['rob_rad']
+plan_dt, ctrl_dt = 1./plan_rate, 1./ctrl_rate
+sim_steps = t_sim * ctrl_rate  # total number of simulation steps
+Kp, k = params['ctrl_speed_gain'], params['ctrl_heading_gain']
+
+# initialize dynamics and controller
+dyn = StanleyState(x=x0, y=y0, yaw=yaw0, v=v0, wb=1, max_steer=np.radians(30.0), dt=ctrl_dt)
+ctrlr = StanleyCtrl(dyn, Kp, k)
+
+# initialize autonomy stack variables
+plan_x, plan_y = [], []  # motion plan
+cx, cy, cyaw = [], [], []  # cubic spline fit of motion plan
+target_idx = 0
+
+# history
+plan_x_original, plan_y_original = [], []
+hist_t = [t]
+hist_x = [ctrlr.state.x]
+hist_y = [ctrlr.state.y]
+
+use_lidar = True
+
+for sim_step in range(sim_steps):
+    print('----------------------')
+    print('t = ', t)
+
+    # perception
+    if use_lidar:
+        print('Sensing environment.')
+        r, ox_lidar, oy_lidar = lidar_sensing(ctrlr.state.x, ctrlr.state.y, env_bounds=params['env_bounds_list'],
+                                ob_list=params['ob_list'], count=360)
+        ox_env, oy_env = params['env_bounds']
+        ox_plan, oy_plan = ox_lidar + ox_env, oy_lidar + oy_env
+    else:
+        ox_plan, oy_plan = params['env']
+    # update planning at
+    if sim_step % plan_rate == 0:
+        print('Motion planning.')
+        t_plan_start = time.time()
+
+        a_star = AStarPlanner(ox_plan, oy_plan, params['grid_size'], params['rob_rad'], params['animate'])
+        plan_x, plan_y = a_star.planning(ctrlr.state.x, ctrlr.state.y, *params['goal'])
+        t_plan_end = time.time()
+        print('A* time: ', t_plan_end - t_plan_start)
+
+        # find control and move vehicle
+        print('Cubic spline fit.')
+        plan_x = plan_x[::-1]
+        plan_y = plan_y[::-1]
+        if len(plan_x) == 1 or at_goal(*params['goal'], ctrlr.state.x, ctrlr.state.y, at_goal_thresh):
+            break
+        cx, cy, cyaw, _, _ = cubic_spline_planner.calc_spline_course(plan_x, plan_y, ds=ctrl_dt)
+        target_idx, _ = ctrlr.calc_target_index(cx, cy)
+
+    # apply control
+    print('Tracking with controller.')
+    a_i = ctrlr.pid_control(target_speed, dyn.v)
+    d_i, target_idx = ctrlr.stanley_control(cx, cy, cyaw, target_idx)
+    ctrlr.state.update(a_i, d_i)
+
+    # increase sim time
+    t += ctrl_dt
+
+    # save data
+    print('Saving data.')
+    hist_t.append(t)
+    hist_x.append(ctrlr.state.x)
+    hist_y.append(ctrlr.state.y)
+
+    # plot
+    print('Plotting.')
+    ax = plt.gca()
+    plt.cla()
+    plot_env(params['start'], params['goal'], params['env_dense'], show=False)
+    plt.scatter(ox_plan, oy_plan, color='hotpink', marker='o')
+    if len(hist_t) == 2:
+        plan_x_original, plan_y_original = plan_x, plan_y
+    plt.plot(plan_x, plan_y, "-r")
+    plt.plot(hist_x, hist_y, "-", color='blue')
+    if in_collision(*params['env'], ctrlr.state.x, ctrlr.state.y, rob_rad):
+        circ = plt.Circle((ctrlr.state.x, ctrlr.state.y), rob_rad, color='tab:blue', ec='tab:red', lw=2)
+    else:
+        circ = plt.Circle((ctrlr.state.x, ctrlr.state.y), rob_rad, color='tab:blue', ec='k', lw=2)
+    ax.add_patch(circ)
+    plt.arrow(ctrlr.state.x, dyn.y, 0.9*rob_rad*np.cos(ctrlr.state.yaw), 0.9*rob_rad*np.sin(ctrlr.state.yaw),
+              width=0.3, head_width=0, color='yellow')
+    plt.pause(ctrl_dt)
+
+
+plt.pause(3.0)
+plt.close()
+
+
+
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/plotting.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/plotting.py
new file mode 100644
index 0000000000..682d6b7399
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/plotting.py
@@ -0,0 +1,17 @@
+"""
+Defines the settings for the plotting functions
+
+Author: Sleiman Safaoui (@The-SS)
+"""
+
+import matplotlib.pyplot as plt
+
+
+def plot_env(start, goal, env, show=True):
+    plt.plot(*env, ".k")
+    plt.plot(*start, "og")
+    plt.plot(*goal, "xb")
+    plt.grid(True)
+    plt.axis("equal")
+    if show:
+        plt.show()
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/settings.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/settings.py
new file mode 100644
index 0000000000..e16881011a
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/settings.py
@@ -0,0 +1,125 @@
+"""
+Defines the settings for the autonomy application.
+This includes defining:
+ - the robot parameters
+ - the simulation parameters including the robot start/goal positions
+ - the environment in which the mobile robot must operate
+
+All units are in meters.
+
+Author: Sleiman Safaoui (@The-SS)
+"""
+import numpy as np
+from plotting import plot_env
+
+
+def sim_params():
+    # plotting
+    show_animation = False
+
+    # autonomy stack update rates
+    planning_rate = 5  # Hz
+    control_rate = 50  # Hz
+
+    # control parameters
+    Kp = 1.0  # speed proportional gain
+    k = 0.5  # control gain
+
+    # robot params
+    robot_radius = 1.0  # m
+    wheelbase = 2 * robot_radius  # m
+    max_speed = 3.0  # m/s
+    max_steer = np.radians(60.0)  # radian
+
+    # initializations
+    sx, sy = 1.5, 1.5  # m
+    gx, gy = 13., 13.  # m
+
+    # environment
+    grid_size = 0.5  # m
+    ox, oy, ox_env, oy_env, ob_list, env_bounds = def_env(ds=grid_size)
+    oxd, oyd, _, _, _, _ = def_env(ds=grid_size, dense=True)
+
+    return {"animate": show_animation,
+            "planning_rate": planning_rate, "control_rate": control_rate,
+            "ctrl_speed_gain": Kp, "ctrl_heading_gain": k,
+            "rob_rad": robot_radius, "wheelbase": wheelbase, "max_speed": max_speed, "max_steer": max_steer,
+            "start": [sx, sy], "goal": [gx, gy],
+            "grid_size": grid_size, "env_bounds": [ox_env, oy_env], "env": [ox, oy], "env_dense": [oxd, oyd],
+            "env_bounds_list": env_bounds, "ob_list": ob_list,
+            }
+
+
+def def_env(ds=0.1, dense=False):
+    """
+    Defines the environment bounds and the obstacles.
+    params:
+        ds: step size
+        dense: if true, ds is divided by 10 (used for improved plotting)
+    returns:
+        ox, oy: lists of x-y pairs indicating that the position is occupied by an obstacle
+    """
+    if dense:
+        ds /= 10
+    # define env bounds
+    xmin, xmax = 0, 15
+    ymin, ymax = 0, 15
+    ox, oy = [], []
+    for i in np.linspace(xmin, xmax, round((xmax - xmin) / ds), endpoint=True):  # bottom
+        ox.append(i)
+        oy.append(ymin)
+    for i in np.linspace(ymin, ymax, round((ymax - ymin) / ds), endpoint=True):  # right
+        ox.append(xmax)
+        oy.append(i)
+    for i in np.linspace(xmin, xmax, round((xmax - xmin) / ds), endpoint=True):  # top
+        ox.append(i)
+        oy.append(ymax)
+    for i in np.linspace(ymin, ymax, round((ymax - ymin) / ds), endpoint=True):  # left
+        ox.append(xmin)
+        oy.append(i)
+    env_bounds = [xmin, xmax, ymin, ymax]
+    ox_env, oy_env = ox[:], oy[:]
+
+    def populate_obst(xmin, xmax, ymin, ymax):
+        """ defines a rectangular obstacle between the given bounds"""
+        for cy in np.linspace(ymin, ymax, round((ymax - ymin) / ds), endpoint=True):
+            for cx in np.linspace(xmin, xmax, round((xmax - xmin) / ds), endpoint=True):
+                ox.append(cx)
+                oy.append(cy)
+
+    # define obstacles: list of [xmin, xmax, ymin, ymax]
+    ob_list = [#[3, 4, 0, 10],
+               [3, 4, 0, 4],
+               [7, 8, 5, 15],
+               [8, 12, 5, 6],
+               [7, 9, 1, 2],
+               [11, 12, 3, 5],
+               [11, 15, 9, 10]]
+    for ob in ob_list:
+        populate_obst(*ob)
+
+    return ox, oy, ox_env, oy_env, ob_list, env_bounds
+
+
+if __name__ == "__main__":
+    params = sim_params()
+    plot_env(params["start"], params["goal"], params["env"])
+    plot_env(params["start"], params["goal"], params["env_dense"])
+
+    # import matplotlib.pyplot as plt
+    # from lidar_sensing import lidar_sensing
+    # plot_env(params["start"], params["goal"], params["env_dense"], show=False)
+    # r, x, y = lidar_sensing(*params['start'], env_bounds=params['env_bounds_list'], ob_list=params['ob_list'], count=360)
+    # plt.scatter(x, y)
+    # plt.show()
+
+    # from lidar_sensing import simulate_lidar, plot_lidar_simulation
+    #
+    # max_range = 10.0  # Maximum range of the LiDAR sensor
+    #
+    # # Simulate LiDAR measurements
+    # lidar_measurements = simulate_lidar(*params["start"], *params["env"], max_range)
+    # # Plot the LiDAR simulation
+    # plot_lidar_simulation(*params["start"], *params["env"], lidar_measurements)
+
+
diff --git a/MobileRobotNavigation/mobile_robot_autonomy_stack/tmp/planning_and_control.py b/MobileRobotNavigation/mobile_robot_autonomy_stack/tmp/planning_and_control.py
new file mode 100644
index 0000000000..0ea5d26f5c
--- /dev/null
+++ b/MobileRobotNavigation/mobile_robot_autonomy_stack/tmp/planning_and_control.py
@@ -0,0 +1,112 @@
+"""
+Planning and control stack for a mobile robot using existing algorithms in PythonRobotics.
+Planning with A*.
+
+Author: Sleiman Safaoui (@The-SS)
+
+"""
+import numpy as np
+
+from settings import sim_params
+from plotting import plot_env
+from PathPlanning.AStar.a_star import AStarPlanner
+import time
+from dynamics import StanleyState, StanleyCtrl
+import matplotlib.pyplot as plt
+from PathPlanning.CubicSpline import cubic_spline_planner
+# from PathTracking.stanley_controller.stanley_controller import pid_control, stanley_control
+from helpers import in_collision, at_goal
+
+
+t = 0  # simulation time
+t_sim = 20  # [sec] simulation time
+at_goal_thresh = 0.3
+
+# load simulation parameters
+params = sim_params()
+x0, y0 = params['start']
+yaw0 = np.radians(90)
+v0 = 0
+wb, max_steer = params['wheelbase'], params['max_steer']
+plan_rate, ctrl_rate = params['planning_rate'], params['control_rate']
+target_speed = params['max_speed']
+rob_rad = params['rob_rad']
+plan_dt, ctrl_dt = 1./plan_rate, 1./ctrl_rate
+sim_steps = t_sim * ctrl_rate  # total number of simulation steps
+Kp, k = params['ctrl_speed_gain'], params['ctrl_heading_gain']
+
+# initialize dynamics and controller
+dyn = StanleyState(x=x0, y=y0, yaw=yaw0, v=v0, wb=1, max_steer=np.radians(30.0), dt=ctrl_dt)
+ctrlr = StanleyCtrl(dyn, Kp, k)
+
+# initialize autonomy stack variables
+plan_x, plan_y = [], []  # motion plan
+cx, cy, cyaw = [], [], []  # cubic spline fit of motion plan
+target_idx = 0
+
+# history
+plan_x_original, plan_y_original = [], []
+hist_t = [t]
+hist_x = [ctrlr.state.x]
+hist_y = [ctrlr.state.y]
+
+for sim_step in range(sim_steps):
+    print('----------------------')
+    print('t = ', t)
+    # update planning at
+    if sim_step % plan_rate == 0:
+        t_plan_start = time.time()
+        a_star = AStarPlanner(*params['env'], params['grid_size'], params['rob_rad'], params['animate'])
+        plan_x, plan_y = a_star.planning(ctrlr.state.x, ctrlr.state.y, *params['goal'])
+        t_plan_end = time.time()
+        print('A* time: ', t_plan_end - t_plan_start)
+
+        plot_env(params['start'], params['goal'], params['env_dense'], show=False)
+        plt.plot(plan_x, plan_y, "-r")
+        plt.show(block=False)
+
+        # find control and move vehicle
+        plan_x = plan_x[::-1]
+        plan_y = plan_y[::-1]
+        if len(plan_x) == 1 or at_goal(*params['goal'], ctrlr.state.x, ctrlr.state.y, at_goal_thresh):
+            break
+        cx, cy, cyaw, _, _ = cubic_spline_planner.calc_spline_course(plan_x, plan_y, ds=ctrl_dt)
+        target_idx, _ = ctrlr.calc_target_index(cx, cy)
+
+    # apply control
+    a_i = ctrlr.pid_control(target_speed, dyn.v)
+    d_i, target_idx = ctrlr.stanley_control(cx, cy, cyaw, target_idx)
+    ctrlr.state.update(a_i, d_i)
+
+    # increase sim time
+    t += ctrl_dt
+
+    # save data
+    hist_t.append(t)
+    hist_x.append(ctrlr.state.x)
+    hist_y.append(ctrlr.state.y)
+
+    # plot
+    plt.cla()
+    ax = plt.gca()
+    plot_env(params['start'], params['goal'], params['env_dense'], show=False)
+    if len(hist_t) == 2:
+        plan_x_original, plan_y_original = plan_x, plan_y
+    plt.plot(plan_x, plan_y, "-r")
+    plt.plot(hist_x, hist_y, "-", color='blue')
+    if in_collision(*params['env'], ctrlr.state.x, ctrlr.state.y, rob_rad):
+        circ = plt.Circle((ctrlr.state.x, ctrlr.state.y), rob_rad, color='tab:blue', ec='tab:red', lw=2)
+    else:
+        circ = plt.Circle((ctrlr.state.x, ctrlr.state.y), rob_rad, color='tab:blue', ec='k', lw=2)
+    ax.add_patch(circ)
+    plt.arrow(ctrlr.state.x, dyn.y, 0.9*rob_rad*np.cos(ctrlr.state.yaw), 0.9*rob_rad*np.sin(ctrlr.state.yaw),
+              width=0.3, head_width=0, color='yellow')
+    plt.pause(ctrl_dt)
+
+
+plt.plot(plan_x_original, plan_y_original, "-r")
+plt.pause(3.0)
+plt.close()
+
+
+
diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py
index 6d20350432..aa4af34c41 100644
--- a/PathPlanning/AStar/a_star.py
+++ b/PathPlanning/AStar/a_star.py
@@ -18,7 +18,7 @@
 
 class AStarPlanner:
 
-    def __init__(self, ox, oy, resolution, rr):
+    def __init__(self, ox, oy, resolution, rr, show_animation):
         """
         Initialize grid map for a star planning
 
@@ -36,6 +36,7 @@ def __init__(self, ox, oy, resolution, rr):
         self.x_width, self.y_width = 0, 0
         self.motion = self.get_motion_model()
         self.calc_obstacle_map(ox, oy)
+        self.show_animation = show_animation
 
     class Node:
         def __init__(self, x, y, cost, parent_index):
@@ -84,7 +85,7 @@ def planning(self, sx, sy, gx, gy):
             current = open_set[c_id]
 
             # show graph
-            if show_animation:  # pragma: no cover
+            if self.show_animation:  # pragma: no cover
                 plt.plot(self.calc_grid_position(current.x, self.min_x),
                          self.calc_grid_position(current.y, self.min_y), "xc")
                 # for stopping simulation with the esc key.
@@ -269,7 +270,7 @@ def main():
         plt.grid(True)
         plt.axis("equal")
 
-    a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
+    a_star = AStarPlanner(ox, oy, grid_size, robot_radius, show_animation)
     rx, ry = a_star.planning(sx, sy, gx, gy)
 
     if show_animation:  # pragma: no cover