diff --git a/Cargo.toml b/Cargo.toml
index 4f5d690..c6b2809 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -14,10 +14,10 @@ openrr-nav = "0.1"
 
 anyhow = "1"
 arci = "0.1"
+arci-urdf-viz = "0.1"
 bevy = "0.11"
 bevy_egui = "0.21"
 image = "0.24"
-nalgebra = "0.32"
 prost = "0.12"
 prost-types = "0.12"
 rand = "0.8"
@@ -29,6 +29,7 @@ tonic-build = "0.10"
 serde = { version = "1.0", features = ["derive"] }
 serde_yaml = "0.9"
 clap = { version = "4.4", features = ["derive", "env"] }
+yaml-rust = "0.4"
 
 [patch.crates-io]
 grid_map = { path = "grid_map" }
diff --git a/openrr-nav-viewer/Cargo.toml b/openrr-nav-viewer/Cargo.toml
index b503104..18593fc 100644
--- a/openrr-nav-viewer/Cargo.toml
+++ b/openrr-nav-viewer/Cargo.toml
@@ -11,10 +11,10 @@ repository.workspace = true
 tonic-build.workspace = true
 
 [dependencies]
+arci.workspace = true
 bevy_egui.workspace = true
 bevy.workspace = true
 grid_map.workspace = true
-nalgebra.workspace = true
 openrr-nav.workspace = true
 prost-types.workspace = true
 prost.workspace = true
@@ -24,6 +24,7 @@ clap.workspace = true
 
 [dev-dependencies]
 anyhow.workspace = true
+arci-urdf-viz.workspace = true
 rand.workspace = true
 rrt.workspace = true
 
diff --git a/openrr-nav-viewer/examples/controller.rs b/openrr-nav-viewer/examples/controller.rs
index 423dab9..f14b5f9 100644
--- a/openrr-nav-viewer/examples/controller.rs
+++ b/openrr-nav-viewer/examples/controller.rs
@@ -79,19 +79,14 @@ async fn controller(
         add_target_position_to_path(result, &Pose::new(Vector2::new(goal[0], goal[1]), goal[2]));
     api.set_global_path(pb::RobotPath::from(robot_path_from_vec_vec(result.clone())))
         .await?;
-    let path_grid = result
-        .iter()
-        .map(|p| map.to_grid(p[0], p[1]).unwrap())
-        .collect::<Vec<_>>();
 
     for p in &result {
         map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
     }
 
-    let path_distance_map = path_distance_map(&map, &path_grid).unwrap();
+    let path_distance_map = path_distance_map(&map, &result).unwrap();
 
-    let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-    let goal_distance_map = goal_distance_map(&map, &goal_grid).unwrap();
+    let goal_distance_map = goal_distance_map(&map, &goal).unwrap();
 
     let obstacle_distance_map = obstacle_distance_map(&map).unwrap();
 
@@ -147,10 +142,9 @@ async fn controller(
     for i in 0..300 {
         // let dynamic_map = new_dynamic_sample_map(i);
         let dynamic_map = new_sample_map();
-        let path_distance_map = openrr_nav::path_distance_map(&dynamic_map, &path_grid).unwrap();
+        let path_distance_map = openrr_nav::path_distance_map(&dynamic_map, &result).unwrap();
 
-        let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-        let goal_distance_map = openrr_nav::goal_distance_map(&dynamic_map, &goal_grid).unwrap();
+        let goal_distance_map = openrr_nav::goal_distance_map(&dynamic_map, &goal).unwrap();
 
         let obstacle_distance_map = openrr_nav::obstacle_distance_map(&dynamic_map).unwrap();
 
diff --git a/openrr-nav-viewer/examples/dwa_gui.rs b/openrr-nav-viewer/examples/dwa_gui.rs
index 57143cb..505aa80 100644
--- a/openrr-nav-viewer/examples/dwa_gui.rs
+++ b/openrr-nav-viewer/examples/dwa_gui.rs
@@ -82,19 +82,14 @@ fn main() {
             let mut locked_robot_path = cloned_nav.robot_path.lock().unwrap();
             locked_robot_path.set_global_path(robot_path_from_vec_vec(result.clone()));
         }
-        let path_grid = result
-            .iter()
-            .map(|p| map.to_grid(p[0], p[1]).unwrap())
-            .collect::<Vec<_>>();
 
         for p in &result {
             map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
         }
 
-        let path_distance_map = path_distance_map(&map, &path_grid).unwrap();
+        let path_distance_map = path_distance_map(&map, &result).unwrap();
 
-        let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-        let goal_distance_map = goal_distance_map(&map, &goal_grid).unwrap();
+        let goal_distance_map = goal_distance_map(&map, &goal).unwrap();
 
         let obstacle_distance_map = obstacle_distance_map(&map).unwrap();
 
@@ -129,12 +124,9 @@ fn main() {
         for i in 0..300 {
             // let dynamic_map = new_dynamic_sample_map(i);
             let dynamic_map = new_sample_map();
-            let path_distance_map =
-                openrr_nav::path_distance_map(&dynamic_map, &path_grid).unwrap();
+            let path_distance_map = openrr_nav::path_distance_map(&dynamic_map, &result).unwrap();
 
-            let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-            let goal_distance_map =
-                openrr_nav::goal_distance_map(&dynamic_map, &goal_grid).unwrap();
+            let goal_distance_map = openrr_nav::goal_distance_map(&dynamic_map, &goal).unwrap();
 
             let obstacle_distance_map = openrr_nav::obstacle_distance_map(&dynamic_map).unwrap();
 
diff --git a/openrr-nav-viewer/examples/shared/mod.rs b/openrr-nav-viewer/examples/shared/mod.rs
index 24d7809..d090947 100644
--- a/openrr-nav-viewer/examples/shared/mod.rs
+++ b/openrr-nav-viewer/examples/shared/mod.rs
@@ -1,6 +1,6 @@
+use arci::nalgebra as na;
 use clap::Parser;
 use grid_map::*;
-use nalgebra as na;
 use openrr_nav::*;
 use openrr_nav_viewer::NavigationViz;
 
diff --git a/openrr-nav-viewer/examples/urdf_viz_client.rs b/openrr-nav-viewer/examples/urdf_viz_client.rs
new file mode 100644
index 0000000..c8cca2d
--- /dev/null
+++ b/openrr-nav-viewer/examples/urdf_viz_client.rs
@@ -0,0 +1,199 @@
+use arci::{nalgebra as na, Localization};
+use arci_urdf_viz::UrdfVizWebClient;
+use grid_map::*;
+use openrr_nav::*;
+use openrr_nav_viewer::{BevyAppNav, NavigationViz};
+use std::sync::{Arc, Mutex};
+
+fn new_sample_map() -> GridMap<u8> {
+    let mut map = grid_map::GridMap::<u8>::new(
+        Position::new(-1.05, -1.05),
+        Position::new(10.05, 10.05),
+        0.1,
+    );
+
+    for i in 0..40 {
+        for j in 0..20 {
+            map.set_obstacle(&Grid {
+                x: 40 + i,
+                y: 30 + j,
+            });
+        }
+    }
+    for i in 0..20 {
+        for j in 0..30 {
+            map.set_obstacle(&Grid {
+                x: 60 + i,
+                y: 65 + j,
+            });
+        }
+    }
+
+    map
+}
+
+fn robot_path_from_vec_vec(path: Vec<Vec<f64>>) -> RobotPath {
+    let mut robot_path_inner = vec![];
+    for p in path {
+        let pose = na::Isometry2::new(na::Vector2::new(p[0], p[1]), 0.);
+
+        robot_path_inner.push(pose);
+    }
+    RobotPath(robot_path_inner)
+}
+
+fn main() {
+    let client = UrdfVizWebClient::default();
+    client.run_send_velocity_thread();
+
+    let planner_config_path = format!(
+        "{}/../openrr-nav/config/dwa_parameter_config.yaml",
+        env!("CARGO_MANIFEST_DIR")
+    );
+    let nav = NavigationViz::new(&planner_config_path).unwrap();
+
+    let start = client.current_pose("").unwrap();
+    let start = [
+        start.translation.x,
+        start.translation.y,
+        start.rotation.angle(),
+    ];
+    let goal = [9.0, 8.0, std::f64::consts::FRAC_PI_3];
+    {
+        let mut locked_start = nav.start_position.lock().unwrap();
+        *locked_start = Pose::new(Vector2::new(start[0], start[1]), start[2]);
+        let mut locked_goal = nav.goal_position.lock().unwrap();
+        *locked_goal = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]);
+    }
+
+    let cloned_nav = nav.clone();
+
+    nav.reload_planner().unwrap();
+
+    let mut local_plan_executor = LocalPlanExecutor::new(
+        Arc::new(Mutex::new(client.clone())),
+        Arc::new(Mutex::new(client.clone())),
+        "".to_owned(),
+        nav.planner.lock().unwrap().clone(),
+        0.1,
+    );
+
+    std::thread::spawn(move || loop {
+        if *cloned_nav.is_run.lock().unwrap() {
+            let mut map = new_sample_map();
+            let start;
+            let goal;
+            {
+                let current_pose = client.current_pose("").unwrap();
+                let mut locked_start = cloned_nav.start_position.lock().unwrap();
+                *locked_start = current_pose;
+                start = [
+                    current_pose.translation.x,
+                    current_pose.translation.y,
+                    current_pose.rotation.angle(),
+                ];
+
+                let locked_goal = cloned_nav.goal_position.lock().unwrap();
+                goal = [
+                    locked_goal.translation.x,
+                    locked_goal.translation.y,
+                    locked_goal.rotation.angle(),
+                ];
+            }
+
+            let mut global_plan = GlobalPlan::new(map.clone(), start, goal);
+
+            let result = global_plan.global_plan();
+            {
+                let mut locked_robot_path = cloned_nav.robot_path.lock().unwrap();
+                locked_robot_path.set_global_path(robot_path_from_vec_vec(result.clone()));
+            }
+
+            let mut cost_maps = CostMaps::new(&map, &result, &start, &goal);
+            let mut angle_table = AngleTable::new(start[2], goal[2]);
+
+            for p in result.iter() {
+                map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
+            }
+
+            local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
+            {
+                let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock().unwrap();
+                *locked_layered_grid_map = cost_maps.layered_grid_map();
+            }
+
+            local_plan_executor.set_angle_table(angle_table.angle_table());
+            {
+                let mut locked_angle_table = cloned_nav.angle_table.lock().unwrap();
+                *locked_angle_table = angle_table.angle_table();
+            }
+
+            let mut current_pose;
+            let goal_pose = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]);
+
+            const STEP: usize = 2000;
+            for i in 0..STEP {
+                current_pose = local_plan_executor.current_pose().unwrap();
+
+                cost_maps.update(
+                    &None,
+                    &result,
+                    &[current_pose.translation.x, current_pose.translation.y],
+                    &[],
+                );
+
+                angle_table.update(Some(current_pose), &result);
+
+                local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
+                {
+                    let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock().unwrap();
+                    *locked_layered_grid_map = cost_maps.layered_grid_map();
+                }
+
+                local_plan_executor.set_angle_table(angle_table.angle_table());
+                {
+                    let mut locked_angle_table = cloned_nav.angle_table.lock().unwrap();
+                    *locked_angle_table = angle_table.angle_table();
+                }
+
+                local_plan_executor.exec_once().unwrap();
+
+                {
+                    let mut locked_robot_pose = cloned_nav.robot_pose.lock().unwrap();
+                    *locked_robot_pose = current_pose;
+                }
+
+                println!(
+                    "[ {:4} / {} ] X: {:.3}, Y: {:.3}, THETA: {:.3}",
+                    i + 1,
+                    STEP,
+                    current_pose.translation.x,
+                    current_pose.translation.y,
+                    current_pose.rotation.angle()
+                );
+                std::thread::sleep(std::time::Duration::from_millis(5));
+
+                const GOAL_THRESHOLD_DISTANCE: f64 = 0.1;
+                const GOAL_THRESHOLD_ANGLE_DIFFERENCE: f64 = 0.4;
+                if (goal_pose.translation.vector - current_pose.translation.vector).norm()
+                    < GOAL_THRESHOLD_DISTANCE
+                    && (goal_pose.rotation.angle() - current_pose.rotation.angle()).abs()
+                        < GOAL_THRESHOLD_ANGLE_DIFFERENCE
+                {
+                    println!("GOAL! count = {i}");
+                    break;
+                }
+            }
+            local_plan_executor.stop().unwrap();
+            {
+                let mut is_run = cloned_nav.is_run.lock().unwrap();
+                *is_run = false;
+            }
+        }
+    });
+
+    let bevy_cloned_nav = nav.clone();
+    let mut app = BevyAppNav::new();
+    app.setup(bevy_cloned_nav);
+    app.run();
+}
diff --git a/openrr-nav-viewer/src/bevy_app.rs b/openrr-nav-viewer/src/bevy_app.rs
index db1f1f3..eae8714 100644
--- a/openrr-nav-viewer/src/bevy_app.rs
+++ b/openrr-nav-viewer/src/bevy_app.rs
@@ -1,3 +1,4 @@
+use arci::nalgebra as na;
 use bevy::{
     prelude::*,
     winit::{UpdateMode, WinitSettings},
@@ -10,7 +11,6 @@ use bevy_egui::{
     },
     EguiContexts, EguiPlugin,
 };
-use nalgebra::Vector2;
 use openrr_nav::Pose;
 
 use crate::*;
@@ -200,7 +200,10 @@ fn update_system(
                     let angle = (p.y - start_position.translation.y)
                         .atan2(p.x - start_position.translation.x);
                     *start_position = Pose::new(
-                        Vector2::new(start_position.translation.x, start_position.translation.y),
+                        na::Vector2::new(
+                            start_position.translation.x,
+                            start_position.translation.y,
+                        ),
                         angle,
                     );
                     println!("start: {:?}", start_position);
@@ -213,7 +216,7 @@ fn update_system(
                     && ui_checkboxes.counter == 0
                 {
                     let mut start_position = res_nav.start_position.lock().unwrap();
-                    *start_position = Pose::new(Vector2::new(p.x, p.y), 0.0);
+                    *start_position = Pose::new(na::Vector2::new(p.x, p.y), 0.0);
                     ui_checkboxes.counter = 1;
                     displayed_arrows.set_start([p.x, p.y]);
                 }
@@ -232,7 +235,7 @@ fn update_system(
                     let angle = (p.y - goal_position.translation.y)
                         .atan2(p.x - goal_position.translation.x);
                     *goal_position = Pose::new(
-                        Vector2::new(goal_position.translation.x, goal_position.translation.y),
+                        na::Vector2::new(goal_position.translation.x, goal_position.translation.y),
                         angle,
                     );
                     println!("goal: {:?}", goal_position);
@@ -247,7 +250,7 @@ fn update_system(
                     && ui_checkboxes.counter == 0
                 {
                     let mut goal_position = res_nav.goal_position.lock().unwrap();
-                    *goal_position = Pose::new(Vector2::new(p.x, p.y), 0.0);
+                    *goal_position = Pose::new(na::Vector2::new(p.x, p.y), 0.0);
                     ui_checkboxes.counter = 1;
                     displayed_arrows.set_start([p.x, p.y]);
                 }
diff --git a/openrr-nav-viewer/src/converter.rs b/openrr-nav-viewer/src/converter.rs
index 7ceb88b..37f4457 100644
--- a/openrr-nav-viewer/src/converter.rs
+++ b/openrr-nav-viewer/src/converter.rs
@@ -1,10 +1,10 @@
+use arci::nalgebra as na;
 use bevy_egui::egui::{
     epaint::Hsva,
     plot::{Line, PlotPoints, Points, Polygon},
     Color32,
 };
 use grid_map::*;
-use nalgebra as na;
 use openrr_nav::*;
 
 pub fn grid_map_to_polygon(grid_map: &GridMap<u8>) -> Vec<Polygon> {
diff --git a/openrr-nav-viewer/src/lib.rs b/openrr-nav-viewer/src/lib.rs
index 34451a6..2f2c3a8 100644
--- a/openrr-nav-viewer/src/lib.rs
+++ b/openrr-nav-viewer/src/lib.rs
@@ -272,8 +272,8 @@ impl From<pb::Cell> for grid_map::Cell<u8> {
     }
 }
 
-impl From<nalgebra::Isometry2<f64>> for pb::Isometry2 {
-    fn from(val: nalgebra::Isometry2<f64>) -> Self {
+impl From<arci::nalgebra::Isometry2<f64>> for pb::Isometry2 {
+    fn from(val: arci::nalgebra::Isometry2<f64>) -> Self {
         Self {
             rotation: Some(pb::UnitComplex {
                 re: val.rotation.re,
@@ -286,13 +286,13 @@ impl From<nalgebra::Isometry2<f64>> for pb::Isometry2 {
         }
     }
 }
-impl From<pb::Isometry2> for nalgebra::Isometry2<f64> {
+impl From<pb::Isometry2> for arci::nalgebra::Isometry2<f64> {
     fn from(val: pb::Isometry2) -> Self {
         let translation = val.translation.unwrap();
         let rotation = val.rotation.unwrap();
         Self::from_parts(
-            nalgebra::Translation2::new(translation.x, translation.y),
-            nalgebra::UnitComplex::from_complex(nalgebra::Complex {
+            arci::nalgebra::Translation2::new(translation.x, translation.y),
+            arci::nalgebra::UnitComplex::from_complex(arci::nalgebra::Complex {
                 re: rotation.re,
                 im: rotation.im,
             }),
diff --git a/openrr-nav-viewer/src/nav_viz.rs b/openrr-nav-viewer/src/nav_viz.rs
index 8cc7d3a..9753f1f 100644
--- a/openrr-nav-viewer/src/nav_viz.rs
+++ b/openrr-nav-viewer/src/nav_viz.rs
@@ -6,7 +6,7 @@ use std::{
     sync::{Arc, Mutex},
 };
 
-#[derive(Debug, Clone, Resource)]
+#[derive(Debug, Clone, Resource, Default)]
 pub struct NavigationViz {
     pub layered_grid_map: Arc<Mutex<LayeredGridMap<u8>>>,
     pub angle_table: Arc<Mutex<HashMap<String, f64>>>,
diff --git a/openrr-nav/Cargo.toml b/openrr-nav/Cargo.toml
index e54a9e6..da4f951 100644
--- a/openrr-nav/Cargo.toml
+++ b/openrr-nav/Cargo.toml
@@ -10,15 +10,15 @@ repository.workspace = true
 [dependencies]
 arci.workspace = true
 grid_map.workspace = true
-nalgebra.workspace = true
+rand.workspace = true
+rrt.workspace = true
 thiserror.workspace = true
 serde.workspace = true
 serde_yaml.workspace = true
+yaml-rust.workspace = true
 
 [dev-dependencies]
 bevy.workspace = true
-rand.workspace = true
-rrt.workspace = true
 
 [lints]
 workspace = true
diff --git a/openrr-nav/examples/dwa.rs b/openrr-nav/examples/dwa.rs
index b14a587..aff08bd 100644
--- a/openrr-nav/examples/dwa.rs
+++ b/openrr-nav/examples/dwa.rs
@@ -41,20 +41,14 @@ fn main() {
     )
     .unwrap();
 
-    let path_grid = result
-        .iter()
-        .map(|p| map.to_grid(p[0], p[1]).unwrap())
-        .collect::<Vec<_>>();
-
-    for p in result {
+    for p in result.iter() {
         map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
     }
     show_ascii_map(&map, 1.0);
-    let path_distance_map = path_distance_map(&map, &path_grid).unwrap();
+    let path_distance_map = path_distance_map(&map, &result).unwrap();
     show_ascii_map(&path_distance_map, 1.0);
     println!("=======================");
-    let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-    let goal_distance_map = goal_distance_map(&map, &goal_grid).unwrap();
+    let goal_distance_map = goal_distance_map(&map, &goal).unwrap();
     show_ascii_map(&goal_distance_map, 0.1);
     println!("=======================");
     let obstacle_distance_map = obstacle_distance_map(&map).unwrap();
diff --git a/openrr-nav/src/angle_table.rs b/openrr-nav/src/angle_table.rs
new file mode 100644
index 0000000..ff8d2a9
--- /dev/null
+++ b/openrr-nav/src/angle_table.rs
@@ -0,0 +1,41 @@
+use std::collections::HashMap;
+
+use crate::{utils::nearest_path_point, Pose};
+
+const ROTATION_COST_NAME: &str = "rotation";
+const PATH_DIRECTION_COST_NAME: &str = "path_direction";
+const GOAL_DIRECTION_COST_NAME: &str = "goal_direction";
+
+#[derive(Debug)]
+pub struct AngleTable(HashMap<String, f64>);
+
+impl AngleTable {
+    pub fn new(start: f64, goal: f64) -> Self {
+        let mut angles = HashMap::new();
+
+        angles.insert(ROTATION_COST_NAME.to_owned(), start);
+        angles.insert(PATH_DIRECTION_COST_NAME.to_owned(), start);
+        angles.insert(GOAL_DIRECTION_COST_NAME.to_owned(), goal);
+
+        Self(angles)
+    }
+
+    pub fn update(&mut self, current_pose: Option<Pose>, path: &[Vec<f64>]) {
+        if let Some(pose) = current_pose {
+            self.0
+                .insert(ROTATION_COST_NAME.to_owned(), pose.rotation.angle());
+
+            if !path.is_empty() {
+                let nearest_path_point =
+                    nearest_path_point(path, [pose.translation.x, pose.translation.y]);
+                if let Some(p) = nearest_path_point {
+                    self.0.insert(PATH_DIRECTION_COST_NAME.to_owned(), p.1[2]);
+                }
+            }
+        }
+    }
+
+    pub fn angle_table(&self) -> HashMap<String, f64> {
+        self.0.clone()
+    }
+}
diff --git a/openrr-nav/src/cost_map.rs b/openrr-nav/src/cost_map.rs
index 6778be4..5f246b5 100644
--- a/openrr-nav/src/cost_map.rs
+++ b/openrr-nav/src/cost_map.rs
@@ -1,16 +1,102 @@
-use grid_map::{Cell, Error, Grid, GridMap, Position, Result};
+use grid_map::{Cell, Error, Grid, GridMap, LayeredGridMap, Position, Result};
 
 use crate::utils::nearest_path_point;
 
+const PATH_DISTANCE_MAP_NAME: &str = "path";
+const GOAL_DISTANCE_MAP_NAME: &str = "goal";
+const OBSTACLE_DISTANCE_MAP_NAME: &str = "obstacle";
+const LOCAL_GOAL_DISTANCE_MAP_NAME: &str = "local_goal";
+
+#[derive(Debug)]
+pub struct CostMaps {
+    original_map: GridMap<u8>,
+    maps: LayeredGridMap<u8>,
+}
+
+impl CostMaps {
+    pub fn new(map: &GridMap<u8>, path: &[Vec<f64>], start: &[f64], goal: &[f64]) -> Self {
+        let mut maps = LayeredGridMap::default();
+
+        maps.add_layer(
+            PATH_DISTANCE_MAP_NAME.to_owned(),
+            path_distance_map(map, path).unwrap(),
+        );
+        maps.add_layer(
+            GOAL_DISTANCE_MAP_NAME.to_owned(),
+            goal_distance_map(map, goal).unwrap(),
+        );
+        maps.add_layer(
+            OBSTACLE_DISTANCE_MAP_NAME.to_owned(),
+            obstacle_distance_map(map).unwrap(),
+        );
+        maps.add_layer(
+            LOCAL_GOAL_DISTANCE_MAP_NAME.to_owned(),
+            local_goal_distance_map(map, path, [start[0], start[1]]).unwrap(),
+        );
+
+        Self {
+            original_map: map.clone(),
+            maps,
+        }
+    }
+
+    pub fn update(
+        &mut self,
+        map: &Option<GridMap<u8>>,
+        path: &[Vec<f64>],
+        current_pose: &[f64],
+        goal: &[f64],
+    ) {
+        if let Some(m) = map {
+            self.original_map = m.clone();
+            self.maps.add_layer(
+                OBSTACLE_DISTANCE_MAP_NAME.to_owned(),
+                obstacle_distance_map(&self.original_map).unwrap(),
+            );
+        }
+        if !path.is_empty() {
+            self.maps.add_layer(
+                PATH_DISTANCE_MAP_NAME.to_owned(),
+                path_distance_map(&self.original_map, path).unwrap(),
+            );
+        }
+        if !goal.is_empty() {
+            self.maps.add_layer(
+                GOAL_DISTANCE_MAP_NAME.to_owned(),
+                goal_distance_map(&self.original_map, goal).unwrap(),
+            );
+        }
+        if !current_pose.is_empty() {
+            self.maps.add_layer(
+                LOCAL_GOAL_DISTANCE_MAP_NAME.to_owned(),
+                local_goal_distance_map(
+                    &self.original_map,
+                    path,
+                    [current_pose[0], current_pose[1]],
+                )
+                .unwrap(),
+            );
+        }
+    }
+
+    pub fn layered_grid_map(&self) -> LayeredGridMap<u8> {
+        self.maps.clone()
+    }
+}
+
 /// Create path distance map
-pub fn path_distance_map(map: &GridMap<u8>, path: &[Grid]) -> Result<GridMap<u8>> {
+pub fn path_distance_map(map: &GridMap<u8>, path: &[Vec<f64>]) -> Result<GridMap<u8>> {
+    let path_grid = path
+        .iter()
+        .map(|p| map.to_grid(p[0], p[1]).unwrap())
+        .collect::<Vec<_>>();
     let mut path_distance_map = map.copy_without_value();
-    for ind in path {
+    for ind in path_grid.iter() {
         path_distance_map
             .set_value(ind, 0)
             .ok_or_else(|| Error::OutOfRangeGrid(*ind))?;
     }
-    expand_distance_map_internal(&mut path_distance_map, path, 0, |v| {
+    expand_distance_map_internal(&mut path_distance_map, &path_grid, 0, |v| {
         if v == u8::MAX {
             u8::MAX
         } else {
@@ -21,12 +107,13 @@ pub fn path_distance_map(map: &GridMap<u8>, path: &[Grid]) -> Result<GridMap<u8>
 }
 
 /// Create goal distance map
-pub fn goal_distance_map(map: &GridMap<u8>, goal: &Grid) -> Result<GridMap<u8>> {
+pub fn goal_distance_map(map: &GridMap<u8>, goal: &[f64]) -> Result<GridMap<u8>> {
+    let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
     let mut goal_distance_map = map.copy_without_value();
     goal_distance_map
-        .set_value(goal, 0)
-        .ok_or_else(|| Error::OutOfRangeGrid(*goal))?;
-    expand_distance_map_internal(&mut goal_distance_map, &[goal.to_owned()], 0, |v| {
+        .set_value(&goal_grid, 0)
+        .ok_or_else(|| Error::OutOfRangeGrid(goal_grid))?;
+    expand_distance_map_internal(&mut goal_distance_map, &[goal_grid], 0, |v| {
         if v == u8::MAX {
             u8::MAX
         } else {
@@ -90,9 +177,8 @@ pub fn local_goal_distance_map(
     );
 
     let local_map = GridMap::<u8>::new(min_point, max_point, resolution);
-    let grid = local_map.to_grid(local_goal[0], local_goal[1]).unwrap();
 
-    goal_distance_map(&local_map, &grid)
+    goal_distance_map(&local_map, &[local_goal[0], local_goal[1]])
 }
 
 pub fn expand_distance_map_internal<F>(
@@ -168,18 +254,13 @@ mod tests {
         )
         .unwrap();
 
-        let path_grid = result
-            .iter()
-            .map(|p| map.to_grid(p[0], p[1]).unwrap())
-            .collect::<Vec<_>>();
-        for p in result {
+        for p in result.iter() {
             map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
         }
 
-        show_ascii_map(&path_distance_map(&map, &path_grid).unwrap(), 1.0);
+        show_ascii_map(&path_distance_map(&map, &result).unwrap(), 1.0);
         println!("=======================");
-        let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-        show_ascii_map(&goal_distance_map(&map, &goal_grid).unwrap(), 1.0);
+        show_ascii_map(&goal_distance_map(&map, &goal).unwrap(), 1.0);
         println!("=======================");
         show_ascii_map(&obstacle_distance_map(&map).unwrap(), 0.1);
     }
diff --git a/openrr-nav/src/dwa_planner.rs b/openrr-nav/src/dwa_planner.rs
index 2eb7200..e7f94aa 100644
--- a/openrr-nav/src/dwa_planner.rs
+++ b/openrr-nav/src/dwa_planner.rs
@@ -1,6 +1,6 @@
+use arci::nalgebra as na;
 use grid_map::{Cell, GridMap, LayeredGridMap, Position};
 pub use na::Vector2;
-use nalgebra as na;
 use serde::{Deserialize, Serialize};
 use std::{collections::HashMap, fs, path::Path};
 
@@ -351,20 +351,14 @@ mod tests {
         )
         .unwrap();
 
-        let path_grid = result
-            .iter()
-            .map(|p| map.to_grid(p[0], p[1]).unwrap())
-            .collect::<Vec<_>>();
-
-        for p in result {
+        for p in result.iter() {
             map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
         }
         show_ascii_map(&map, 1.0);
-        let path_distance_map = path_distance_map(&map, &path_grid).unwrap();
+        let path_distance_map = path_distance_map(&map, &result).unwrap();
         show_ascii_map(&path_distance_map, 1.0);
         println!("=======================");
-        let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
-        let goal_distance_map = goal_distance_map(&map, &goal_grid).unwrap();
+        let goal_distance_map = goal_distance_map(&map, &goal).unwrap();
         show_ascii_map(&goal_distance_map, 0.1);
         println!("=======================");
         let obstacle_distance_map = obstacle_distance_map(&map).unwrap();
diff --git a/openrr-nav/src/global_plan.rs b/openrr-nav/src/global_plan.rs
new file mode 100644
index 0000000..4fb1d85
--- /dev/null
+++ b/openrr-nav/src/global_plan.rs
@@ -0,0 +1,111 @@
+use arci::nalgebra as na;
+use grid_map::{Cell, GridMap};
+use rand::{distributions::Uniform, prelude::Distribution};
+
+use crate::Pose;
+
+#[derive(Debug)]
+pub struct GlobalPlan {
+    map: GridMap<u8>,
+    start: [f64; 3],
+    goal: [f64; 3],
+}
+
+impl GlobalPlan {
+    pub fn new(map: GridMap<u8>, start: [f64; 3], goal: [f64; 3]) -> Self {
+        Self { map, start, goal }
+    }
+
+    pub fn global_plan(&mut self) -> Vec<Vec<f64>> {
+        let x_range = Uniform::new(self.map.min_point().x, self.map.max_point().x);
+        let y_range = Uniform::new(self.map.min_point().y, self.map.max_point().y);
+
+        let is_free = |p: &[f64]| {
+            !matches!(
+                self.map
+                    .cell(&self.map.to_grid(p[0], p[1]).unwrap())
+                    .unwrap(),
+                Cell::Obstacle
+            )
+        };
+
+        const EXTEND_LENGTH: f64 = 0.05;
+
+        let mut result = rrt::dual_rrt_connect(
+            &[self.start[0], self.start[1]],
+            &[self.goal[0], self.goal[1]],
+            is_free,
+            || {
+                let mut rng = rand::thread_rng();
+                vec![x_range.sample(&mut rng), y_range.sample(&mut rng)]
+            },
+            EXTEND_LENGTH,
+            4000,
+        )
+        .unwrap();
+
+        rrt::smooth_path(&mut result, is_free, EXTEND_LENGTH, 1000);
+
+        let result = linear_interpolate_path(result, EXTEND_LENGTH);
+        add_target_position_to_path(
+            result,
+            &Pose::new(na::Vector2::new(self.goal[0], self.goal[1]), self.goal[2]),
+        )
+    }
+}
+
+fn linear_interpolate_path(path: Vec<Vec<f64>>, extend_length: f64) -> Vec<Vec<f64>> {
+    if path.len() < 2 {
+        return path;
+    }
+    let mut interpolated_path = vec![];
+    for (p0, p1) in path.iter().zip(path.iter().skip(1)) {
+        let diff_x = p1[0] - p0[0];
+        let diff_y = p1[1] - p0[1];
+        let diff = (diff_x.powi(2) + diff_y.powi(2)).sqrt();
+        let direction = diff_y.atan2(diff_x);
+        let interpolate_num = (diff / extend_length) as usize;
+        if interpolate_num > 0 {
+            let unit_diff_x = diff_x / interpolate_num as f64;
+            let unit_diff_y = diff_y / interpolate_num as f64;
+            for j in 1..interpolate_num {
+                interpolated_path.push(vec![
+                    p0[0] + unit_diff_x * j as f64,
+                    p0[1] + unit_diff_y * j as f64,
+                    direction,
+                ]);
+            }
+        } else {
+            interpolated_path.push({
+                let mut p = p0.to_owned();
+                p.push(direction);
+                p
+            });
+        }
+    }
+    let last_point_angle = interpolated_path.last().unwrap_or(&vec![0.; 3])[2];
+    interpolated_path.push({
+        let mut end_path = path.last().unwrap().clone();
+        end_path.push(last_point_angle);
+        end_path
+    });
+    interpolated_path
+}
+
+fn add_target_position_to_path(path: Vec<Vec<f64>>, target_pose: &Pose) -> Vec<Vec<f64>> {
+    let mut p = path.clone();
+    let target_pose_vec = vec![
+        target_pose.translation.x,
+        target_pose.translation.y,
+        target_pose.rotation.angle(),
+    ];
+    match p.last_mut() {
+        Some(v) => {
+            *v = target_pose_vec;
+        }
+        None => {
+            p.push(target_pose_vec);
+        }
+    }
+    p
+}
diff --git a/openrr-nav/src/lib.rs b/openrr-nav/src/lib.rs
index 2515700..7aac9f3 100644
--- a/openrr-nav/src/lib.rs
+++ b/openrr-nav/src/lib.rs
@@ -1,12 +1,18 @@
 // mod angle_table;
+mod angle_table;
 mod cost_map;
 mod dwa_planner;
 mod error;
+mod global_plan;
+mod local_plan_executor;
 mod robot_path;
 pub mod utils;
 
 // pub use crate::angle_table::*;
+pub use crate::angle_table::*;
 pub use crate::cost_map::*;
 pub use crate::dwa_planner::*;
 pub use crate::error::*;
+pub use crate::global_plan::*;
+pub use crate::local_plan_executor::*;
 pub use crate::robot_path::*;
diff --git a/openrr-nav/src/local_plan_executor.rs b/openrr-nav/src/local_plan_executor.rs
index c2e657b..aac8b42 100644
--- a/openrr-nav/src/local_plan_executor.rs
+++ b/openrr-nav/src/local_plan_executor.rs
@@ -1,30 +1,155 @@
-use crate::DwaPlanner;
-use crate::{path_distance_map, obstacle_distance_map, goal_distance_map};
-use arci::MoveBase;
-use std::collections::HashMap;
-use grid_map::{LayeredGridMap};
+use crate::{utils::PoseTimeStamped, DwaPlanner, Error, Result, Velocity};
+use arci::{BaseVelocity, Isometry2, Localization, MoveBase};
+use grid_map::{GridMap, LayeredGridMap};
+use std::{
+    collections::HashMap,
+    sync::{Arc, Mutex},
+};
 
-pub struct LocalPlanExecutor<T> where T: MoveBase {
+#[derive(Debug)]
+pub struct LocalPlanExecutor<M, L>
+where
+    M: MoveBase,
+    L: Localization,
+{
     /// local planner
     /// TODO: Support more planners
-    move_base: T,
+    move_base: Arc<Mutex<M>>,
+    localization: Arc<Mutex<L>>,
+    frame_id: String,
     local_planner: DwaPlanner,
     cost_maps: LayeredGridMap<u8>,
-    weights: HashMap<String, f64>,
+    angle_table: HashMap<String, f64>,
     goal_threshold: f64,
+    last_pose: PoseTimeStamped,
 }
 
-impl<T> LocalPlanExecutor<T> where T: MoveBase {
-    pub fn new(move_base: T, local_planner: DwaPlanner, weights: HashMap<String, f64>, goal_threshold: f64) -> Self {
+impl<M, L> LocalPlanExecutor<M, L>
+where
+    M: MoveBase,
+    L: Localization,
+{
+    pub fn new(
+        move_base: Arc<Mutex<M>>,
+        localization: Arc<Mutex<L>>,
+        frame_id: String,
+        local_planner: DwaPlanner,
+        goal_threshold: f64,
+    ) -> Self {
         Self {
             move_base,
+            localization,
+            frame_id,
             local_planner,
-            weights,
             cost_maps: LayeredGridMap::default(),
+            angle_table: HashMap::default(),
             goal_threshold,
+            last_pose: PoseTimeStamped::default(),
         }
     }
-    pub fn exec_once() -> Result<()> {
+    pub fn set_cost_maps(&mut self, cost_maps: LayeredGridMap<u8>) {
+        self.cost_maps = cost_maps;
+    }
+
+    pub fn set_cost_map_element(&mut self, key: String, value: GridMap<u8>) {
+        self.cost_maps.add_layer(key, value);
+    }
+
+    pub fn set_angle_table(&mut self, angle_table: HashMap<String, f64>) {
+        self.angle_table = angle_table;
+    }
+
+    pub fn set_angle_table_element(&mut self, key: String, value: f64) {
+        self.angle_table.insert(key, value);
+    }
+
+    pub fn exec_once(&mut self) -> Result<()> {
+        let current_pose = self
+            .localization
+            .lock()
+            .unwrap()
+            .current_pose(&self.frame_id)
+            .unwrap();
+
+        let current_velocity = {
+            let v = self.current_velocity().unwrap_or(BaseVelocity {
+                x: 0.,
+                y: 0.,
+                theta: 0.,
+            });
+            Velocity {
+                x: v.x,
+                theta: v.theta,
+            }
+        };
+
+        let plan = self.local_planner.plan_local_path(
+            &current_pose,
+            &current_velocity,
+            &self.cost_maps,
+            &self.angle_table,
+        );
+
+        self.move_base
+            .lock()
+            .unwrap()
+            .send_velocity(&BaseVelocity {
+                x: plan.velocity.x,
+                y: 0.,
+                theta: plan.velocity.theta,
+            })
+            .unwrap();
+
+        self.last_pose.set_pose(current_pose);
 
+        Ok(())
     }
-}
\ No newline at end of file
+
+    pub fn is_reached_to_goal(&self, goal_pose: Isometry2<f64>) -> bool {
+        (goal_pose.translation.vector - self.current_pose().unwrap().translation.vector).norm()
+            < self.goal_threshold
+    }
+
+    pub fn current_velocity(&self) -> Result<BaseVelocity> {
+        match self.move_base.lock().unwrap().current_velocity() {
+            Ok(v) => Ok(v),
+            Err(_) => match self.current_pose() {
+                Ok(current_pose) => {
+                    if self.last_pose.is_initialized() {
+                        let last_pose = self.last_pose.pose().unwrap();
+
+                        let dt = self.last_pose.elapsed().as_secs_f64();
+                        let dx = current_pose.translation.x - last_pose.translation.x;
+                        let dy = current_pose.translation.y - last_pose.translation.y;
+                        let dtheta = current_pose.rotation.angle() - last_pose.rotation.angle();
+
+                        Ok(BaseVelocity {
+                            x: (dx.powi(2) + dy.powi(2)).sqrt() / dt,
+                            y: 0.,
+                            theta: dtheta / dt,
+                        })
+                    } else {
+                        Err(Error::Other("Cannot get current velocity".to_owned()))
+                    }
+                }
+                Err(_) => Err(Error::Other("Cannot get current velocity.".to_owned())),
+            },
+        }
+    }
+
+    pub fn current_pose(&self) -> Result<Isometry2<f64>> {
+        self.localization
+            .lock()
+            .unwrap()
+            .current_pose(&self.frame_id)
+            .map_err(|e| Error::Other(e.to_string()))
+    }
+
+    pub fn stop(&self) -> Result<()> {
+        self.move_base
+            .lock()
+            .unwrap()
+            .send_velocity(&BaseVelocity::new(0., 0., 0.))
+            .map_err(|e| Error::Other(e.to_string()))
+    }
+}
diff --git a/openrr-nav/src/robot_path.rs b/openrr-nav/src/robot_path.rs
index 23182ee..e4705cc 100644
--- a/openrr-nav/src/robot_path.rs
+++ b/openrr-nav/src/robot_path.rs
@@ -1,8 +1,8 @@
-use nalgebra::Isometry2;
+use arci::nalgebra as na;
 use std::collections::HashMap;
 
 #[derive(Debug, Clone, Default)]
-pub struct RobotPath(pub Vec<Isometry2<f64>>);
+pub struct RobotPath(pub Vec<na::Isometry2<f64>>);
 
 impl RobotPath {
     pub fn new() -> Self {
@@ -13,7 +13,7 @@ impl RobotPath {
         self.0.clear();
     }
 
-    pub fn push(&mut self, pose: Isometry2<f64>) {
+    pub fn push(&mut self, pose: na::Isometry2<f64>) {
         self.0.push(pose);
     }
 }
diff --git a/openrr-nav/src/utils.rs b/openrr-nav/src/utils.rs
index d2c9bb1..1daf55b 100644
--- a/openrr-nav/src/utils.rs
+++ b/openrr-nav/src/utils.rs
@@ -1,3 +1,6 @@
+use crate::{Error, Result};
+use arci::nalgebra as na;
+
 /// Utility for debug
 pub fn show_ascii_map(map: &grid_map::GridMap<u8>, scale: f32) {
     use grid_map::Cell;
@@ -37,3 +40,42 @@ pub fn nearest_path_point(path: &[Vec<f64>], target_point: [f64; 2]) -> Option<(
         Some((nearest.0, path[nearest.0].clone()))
     }
 }
+
+#[derive(Debug)]
+pub struct PoseTimeStamped {
+    pose: Option<na::Isometry2<f64>>,
+    time_stamp: std::time::Instant,
+}
+
+impl PoseTimeStamped {
+    pub fn pose(&self) -> Result<na::Isometry2<f64>> {
+        match self.pose {
+            Some(p) => Ok(p),
+            None => Err(Error::Other(
+                "PoseTimeStamped is not initialized!".to_owned(),
+            )),
+        }
+    }
+
+    pub fn set_pose(&mut self, pose: na::Isometry2<f64>) {
+        self.pose = Some(pose);
+        self.time_stamp = std::time::Instant::now();
+    }
+
+    pub fn elapsed(&self) -> std::time::Duration {
+        self.time_stamp.elapsed()
+    }
+
+    pub fn is_initialized(&self) -> bool {
+        self.pose.is_some()
+    }
+}
+
+impl Default for PoseTimeStamped {
+    fn default() -> Self {
+        Self {
+            pose: None,
+            time_stamp: std::time::Instant::now(),
+        }
+    }
+}