diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt
index ba766d3..019eb53 100644
--- a/waypoint_nav/CMakeLists.txt
+++ b/waypoint_nav/CMakeLists.txt
@@ -16,6 +16,8 @@
   actionlib
   tf
   std_srvs
+  dynamic_reconfigure
+  dwa_local_planner
 )
 
 
diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml
index 74eb2df..c747056 100644
--- a/waypoint_nav/package.xml
+++ b/waypoint_nav/package.xml
@@ -27,6 +27,8 @@
   <build_depend>actionlib</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>std_srvs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>dwa_local_planner</build_depend>
   <build_depend>waypoint_saver</build_depend>
   
   <exec_depend>move_base</exec_depend>
@@ -37,6 +39,9 @@
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>actionlib</exec_depend>
   <exec_depend>tf</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>dwa_local_planner</exec_depend>
+  <exec_depend>waypoint_saver</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml
index ec22d02..6dd5e0f 100644
--- a/waypoint_nav/param/waypoints.yaml
+++ b/waypoint_nav/param/waypoints.yaml
@@ -1,8 +1,8 @@
 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.5, rad: 0.2}
+- 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.515219, y: 2.015219, z: 0.0, vel: 1.0, rad: 0.5}
+- 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}
   pose:
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index dbcc69e..880d0a2 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -11,6 +11,9 @@
 #include <tf/transform_listener.h>
 #include <visualization_msgs/MarkerArray.h>
 
+#include <dynamic_reconfigure/client.h>
+#include "dwa_local_planner/DWAPlannerConfig.h"
+
 #include <yaml-cpp/yaml.h>
 #include <waypoint_saver.h>  // include Waypoint class
 
@@ -44,7 +47,9 @@
         move_base_action_("move_base", true),
         rate_(10),
         last_moved_time_(0),
-        dist_err_(0.8)
+        dist_err_(0.8),
+        standard_max_vel_(1.0),
+        min_vel_(0.0)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -69,6 +74,7 @@
                 computeWpOrientation();
             }
             current_waypoint_ = waypoints_.poses.begin();
+            current_wp_num_ = 0;
         } else {
             ROS_ERROR("waypoints file doesn't have name");
         }
@@ -86,6 +92,10 @@
         cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
         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_);
+        ROS_INFO("%f", standard_max_vel_);
     }
 
 
@@ -197,6 +207,7 @@
             sleep();
         }
         current_waypoint_ = waypoints_.poses.begin();
+        current_wp_num_ = 0;
         has_activate_ = true;
         response.success = true;
         return true;
@@ -417,6 +428,7 @@
         move_base_goal.target_pose.header.frame_id = world_frame_;
         move_base_goal.target_pose.pose.position = dest.position;
         move_base_goal.target_pose.pose.orientation = dest.orientation;
+        setMaxVel();
         move_base_action_.sendGoal(move_base_goal);
     }
 
@@ -429,6 +441,19 @@
     }
 
 
+    void setMaxVel()
+    {
+        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;
+        }
+        config_.max_vel_trans = new_max_vel;
+        client_.setConfiguration(config_);
+        ROS_INFO("Set max velocity: %f", new_max_vel);
+    }
+
+
 
     void run()
     {
@@ -447,6 +472,7 @@
                     startNavigationGL(*current_waypoint_);
                     int resend_goal = 0;
                     double start_nav_time = ros::Time::now().toSec();
+                    dist_err_ = waypoint_list_[current_wp_num_].get_rad();
                     while(!onNavigationPoint(current_waypoint_->position, dist_err_)) {
                         if(!has_activate_) throw SwitchRunningStatus();
 
@@ -460,6 +486,7 @@
                             if(resend_goal == 3) {
                                 ROS_WARN("Skip waypoint.");
                                 current_waypoint_++;
+                                current_wp_num_++;
                                 startNavigationGL(*current_waypoint_);
                             }
                             start_nav_time = time;
@@ -468,6 +495,7 @@
                     }
 
                     current_waypoint_++;
+                    current_wp_num_++;
                     if(current_waypoint_ == finish_pose_) {
                         startNavigationGL(*current_waypoint_);
                         while(!navigationFinished() && ros::ok()) sleep();
@@ -484,6 +512,7 @@
 
 
 
+
 private:
     actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
     geometry_msgs::PoseArray waypoints_;
@@ -501,6 +530,11 @@
     ros::Publisher wp_pub_;
     ros::ServiceClient clear_costmaps_srv_;
     double last_moved_time_, dist_err_;
+    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_;
 };
 
 
@@ -508,7 +542,7 @@
 
 
 
-int main(int argc, char *argv[]){
+int main(int argc, char *argv[]) {
     ros::init(argc, argv, ROS_PACKAGE_NAME);
     WaypointsNavigation w_nav;
     w_nav.run();