diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt
index 019eb53..ae594e5 100644
--- a/waypoint_nav/CMakeLists.txt
+++ b/waypoint_nav/CMakeLists.txt
@@ -17,6 +17,7 @@
   tf
   std_srvs
   dynamic_reconfigure
+  base_local_planner
   dwa_local_planner
 )
 
diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index 8bef4db..140a92d 100644
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -1,11 +1,9 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="waypoints_file" default="$(find waypoint_nav)/param/waypoints.yaml"/>
-    <arg name="dist_error" default="0.8"/>
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
         <param name="filename" value="$(arg waypoints_file)"/>
-        <param name="dist_err" value="$(arg dist_error)"/>
     </node>
 
 </launch>
\ No newline at end of file
diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml
index c747056..4c9f5ff 100644
--- a/waypoint_nav/package.xml
+++ b/waypoint_nav/package.xml
@@ -28,6 +28,7 @@
   <build_depend>tf</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>base_local_planner</build_depend>
   <build_depend>dwa_local_planner</build_depend>
   <build_depend>waypoint_saver</build_depend>
   
@@ -40,6 +41,7 @@
   <exec_depend>actionlib</exec_depend>
   <exec_depend>tf</exec_depend>
   <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>base_local_planner</exec_depend>
   <exec_depend>dwa_local_planner</exec_depend>
   <exec_depend>waypoint_saver</exec_depend>
 
diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml
index 6dd5e0f..662eebc 100644
--- a/waypoint_nav/param/waypoints.yaml
+++ b/waypoint_nav/param/waypoints.yaml
@@ -1,7 +1,7 @@
 waypoints:
 - point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5}
-- point: {x: 0.45187, y: -0.576395, z: 0.0, vel: 0.75, rad: 0.2}
-- point: {x: 0.66645, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3}
+- point: {x: 0.45187, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.2}
+- point: {x: 0.66645, y: 0.504773, z: 0.0, vel: 0.3, rad: 0.3}
 - point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 0.8, rad: 0.3}
 finish_pose:
   header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
diff --git a/waypoint_nav/rviz_cfg/nav.rviz b/waypoint_nav/rviz_cfg/nav.rviz
index 8c94ae3..cc1fea4 100644
--- a/waypoint_nav/rviz_cfg/nav.rviz
+++ b/waypoint_nav/rviz_cfg/nav.rviz
@@ -120,7 +120,7 @@
       Axis: Z
       Channel Name: intensity
       Class: rviz/LaserScan
-      Color: 127; 0; 127
+      Color: 0; 255; 0
       Color Transformer: FlatColor
       Decay Time: 0
       Enabled: true
@@ -131,7 +131,7 @@
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 10
+      Size (Pixels): 5
       Size (m): 0.10000000149011612
       Style: Points
       Topic: /scan
@@ -301,7 +301,7 @@
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 10.579483032226562
+      Distance: 11.816969871520996
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -309,8 +309,8 @@
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.2993233799934387
-        Y: 0.4264318346977234
+        X: -0.31653428077697754
+        Y: -0.3555675148963928
         Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
@@ -319,7 +319,7 @@
       Near Clip Distance: 0.009999999776482582
       Pitch: 1.5697963237762451
       Target Frame: base_link
-      Yaw: 3.115396738052368
+      Yaw: 3.145397424697876
     Saved: ~
 Window Geometry:
   Displays:
@@ -327,7 +327,7 @@
   Height: 847
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000193000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000290000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002d40000005e0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000026c00fffffffb0000000800540069006d0065010000000000000450000000000000000000000447000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000193000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000290000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002d40000005e0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000046900fffffffb0000000800540069006d0065010000000000000450000000000000000000000447000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   StateTriggerPanel:
@@ -339,5 +339,5 @@
   Views:
     collapsed: true
   Width: 1504
-  X: 25
-  Y: 1
+  X: 24
+  Y: -19
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 880d0a2..5c67d44 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -12,6 +12,7 @@
 #include <visualization_msgs/MarkerArray.h>
 
 #include <dynamic_reconfigure/client.h>
+#include "base_local_planner/BaseLocalPlannerConfig.h"
 #include "dwa_local_planner/DWAPlannerConfig.h"
 
 #include <yaml-cpp/yaml.h>
@@ -48,6 +49,7 @@
         rate_(10),
         last_moved_time_(0),
         dist_err_(0.8),
+        planner_("base_local_planner/TrajectoryPlannerROS"),
         standard_max_vel_(1.0),
         min_vel_(0.0)
     {
@@ -79,8 +81,6 @@
             ROS_ERROR("waypoints file doesn't have name");
         }
 
-        private_nh.param("dist_err", dist_err_, dist_err_);
-        
         ros::NodeHandle nh;
         start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
         pause_server_ = nh.advertiseService("pause_wp_nav", &WaypointsNavigation::pauseNavigationCallback,this);
@@ -93,8 +93,14 @@
         wp_pub_ = nh.advertise<geometry_msgs::PoseArray>("waypoints", 10);
         clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
 
-        nh.param("/move_base/DWAPlannerROS/max_vel_trans", standard_max_vel_, standard_max_vel_);
-        nh.param("/move_base/DWAPlannerROS/min_vel_trans", min_vel_, min_vel_);
+        nh.param("/move_base/base_local_planner", planner_, planner_);
+        if (planner_ == "base_local_planner/TrajectoryPlannerROS") {
+            nh.param("/move_base/TrajectoryPlannerROS/max_vel_x", standard_max_vel_, standard_max_vel_);
+            nh.param("/move_base/TrajectoryPlannerROS/min_vel_x", min_vel_, min_vel_);
+        } else if (planner_ == "dwa_local_planner/DWAPlannerROS") {
+            nh.param("/move_base/DWAPlannerROS/max_vel_trans", standard_max_vel_, standard_max_vel_);
+            nh.param("/move_base/DWAPlannerROS/min_vel_trans", min_vel_, min_vel_);
+        }
         ROS_INFO("%f", standard_max_vel_);
     }
 
@@ -446,10 +452,18 @@
         float vel_rate = waypoint_list_[current_wp_num_].get_vel();
         double new_max_vel = standard_max_vel_ * vel_rate;
         if(new_max_vel < min_vel_) {
-            new_max_vel = min_vel_ + 0.01;
+            new_max_vel = min_vel_;
         }
-        config_.max_vel_trans = new_max_vel;
-        client_.setConfiguration(config_);
+        dynamic_reconfigure::Client<base_local_planner::BaseLocalPlannerConfig> client{"/move_base/TrajectoryPlannerROS/"};
+        base_local_planner::BaseLocalPlannerConfig config;
+        if (planner_ == "base_local_planner/TrajectoryPlannerROS") {
+            config.max_vel_x = new_max_vel;
+        } else if (planner_ == "dwa_local_planner/DWAPlannerROS") {
+            dynamic_reconfigure::Client<dwa_local_planner::DWAPlannerConfig> client{"/move_base/DWAPlannerROS/"};
+            dwa_local_planner::DWAPlannerConfig config;
+            config.max_vel_trans = new_max_vel;
+        }
+        client.setConfiguration(config);
         ROS_INFO("Set max velocity: %f", new_max_vel);
     }
 
@@ -522,19 +536,15 @@
     std::vector<geometry_msgs::Pose>::iterator last_waypoint_;
     std::vector<geometry_msgs::Pose>::iterator finish_pose_;
     bool has_activate_;
-    std::string robot_frame_, world_frame_;
+    std::string robot_frame_, world_frame_, planner_;
     tf::TransformListener tf_listener_;
     ros::Rate rate_;
     ros::ServiceServer start_server_, pause_server_, unpause_server_, stop_server_, suspend_server_, resume_server_ ,search_server_;
     ros::Subscriber cmd_vel_sub_;
     ros::Publisher wp_pub_;
     ros::ServiceClient clear_costmaps_srv_;
-    double last_moved_time_, dist_err_;
+    double last_moved_time_, dist_err_, standard_max_vel_, min_vel_;
     unsigned short int current_wp_num_;
-    dynamic_reconfigure::Client<dwa_local_planner::DWAPlannerConfig> client_{"/move_base/DWAPlannerROS/"};
-    dwa_local_planner::DWAPlannerConfig config_;
-    double standard_max_vel_;
-    double min_vel_;
 };