diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt
index ae594e5..b4c1003 100644
--- a/waypoint_nav/CMakeLists.txt
+++ b/waypoint_nav/CMakeLists.txt
@@ -19,6 +19,7 @@
   dynamic_reconfigure
   base_local_planner
   dwa_local_planner
+  std_msgs
 )
 
 
@@ -52,3 +53,9 @@
   ${yaml-cpp_LIBRARIES}
 )
 
+add_executable(velocity_controller src/velocity_controller.cpp)
+
+target_link_libraries(velocity_controller
+  ${catkin_LIBRARIES}
+)
+
diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch
index 140a92d..4c19747 100644
--- a/waypoint_nav/launch/waypoint_nav.launch
+++ b/waypoint_nav/launch/waypoint_nav.launch
@@ -1,9 +1,18 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="waypoints_file" default="$(find waypoint_nav)/param/waypoints.yaml"/>
+    <arg name="cmd_vel_topic_in" default="/move_base/cmd_vel"/>
+    <arg name="cmd_vel_topic_out" default="/wp_nav/cmd_vel"/>
+
 
     <node name="waypoint_nav" pkg="waypoint_nav" type="waypoint_nav" output="screen">
         <param name="filename" value="$(arg waypoints_file)"/>
+        <remap from="/cmd_vel" to="$(arg cmd_vel_topic_out)"/>
+    </node>
+
+    <node name="velocity_controller" pkg="waypoint_nav" type="velocity_controller" output="screen">
+        <remap from="/cmd_vel_in" to="$(arg cmd_vel_topic_in)"/>
+        <remap from="/cmd_vel_out" to="$(arg cmd_vel_topic_out)"/>
     </node>
 
 </launch>
\ No newline at end of file
diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml
index 4c9f5ff..b993d36 100644
--- a/waypoint_nav/package.xml
+++ b/waypoint_nav/package.xml
@@ -31,6 +31,7 @@
   <build_depend>base_local_planner</build_depend>
   <build_depend>dwa_local_planner</build_depend>
   <build_depend>waypoint_saver</build_depend>
+  <build_depend>std_msgs</build_depend>
   
   <exec_depend>move_base</exec_depend>
   <exec_depend>yaml-cpp</exec_depend>
@@ -44,6 +45,7 @@
   <exec_depend>base_local_planner</exec_depend>
   <exec_depend>dwa_local_planner</exec_depend>
   <exec_depend>waypoint_saver</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml
index 662eebc..718999c 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.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}
+- point: {x: 0.45187, y: -0.576395, z: 0.0, vel: 0.75, rad: 0.3}
+- 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: 0.8, rad: 0.8}
 finish_pose:
   header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link}
   pose:
diff --git a/waypoint_nav/src/velocity_controller.cpp b/waypoint_nav/src/velocity_controller.cpp
new file mode 100644
index 0000000..fe4ce2d
--- /dev/null
+++ b/waypoint_nav/src/velocity_controller.cpp
@@ -0,0 +1,81 @@
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Twist.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+
+#include <string>
+
+
+class VelocityController {
+public:
+    VelocityController() :
+        move_base_action_("move_base", true),
+        standard_vel_(1.0),
+        current_max_vel_(1.0),
+        min_vel_(0.0)
+    {
+        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) {
+            ROS_INFO("Waiting...");
+        }
+
+        ros::NodeHandle nh;
+        max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this);
+        cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this);
+        cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_out", 10);
+
+        std::string planner = "";
+        std::string param_max, param_min;
+        nh.param("/move_base/base_local_planner", planner, planner);
+        if (planner == "base_local_planner/TrajectoryPlannerROS") {
+            param_max = "/move_base/TrajectoryPlannerROS/max_vel_x";
+            param_min = "/move_base/TrajectoryPlannerROS/min_vel_x";
+        } else if (planner == "dwa_local_planner/DWAPlannerROS") {
+            param_max = "/move_base/DWAPlannerROS/max_vel_trans";
+            param_min = "/move_base/DWAPlannerROS/min_vel_trans";
+        }
+        nh.param(param_max, standard_vel_, standard_vel_);
+        nh.param(param_min, min_vel_, min_vel_);
+        ROS_INFO_STREAM("Standard max vel: " << standard_vel_);
+    }
+
+
+    void maxVelCallback(const std_msgs::Float32 &msg)
+    {
+        current_max_vel_ = standard_vel_ * msg.data;
+        current_max_vel_ = std::max(current_max_vel_, min_vel_);
+        ROS_INFO_STREAM("Set max vel: " << current_max_vel_);
+    }
+
+
+    void cmdVelCallback(const geometry_msgs::Twist &msg)
+    {
+        pub_msg_.linear.x = std::min(float(msg.linear.x), current_max_vel_);
+        pub_msg_.angular = msg.angular;
+        cmd_vel_pub_.publish(pub_msg_);
+    }
+
+
+
+private:
+    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
+    ros::Subscriber max_vel_sub_;
+    ros::Subscriber cmd_vel_sub_;
+    ros::Publisher cmd_vel_pub_;
+    float standard_vel_, current_max_vel_, min_vel_;
+    geometry_msgs::Twist pub_msg_;
+};
+
+
+
+
+
+
+
+int main(int argc, char *argv[]) {
+    ros::init(argc, argv, "velocity_controller");
+    VelocityController vc;
+
+    ros::spin();
+    return 0;
+}
\ No newline at end of file
diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 5c67d44..afa6b81 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -1,4 +1,5 @@
 #include <ros/ros.h>
+#include <std_msgs/Float32.h>
 #include <std_srvs/Trigger.h>
 #include <std_srvs/Empty.h>
 #include <geometry_msgs/Pose.h>
@@ -48,10 +49,7 @@
         move_base_action_("move_base", true),
         rate_(10),
         last_moved_time_(0),
-        dist_err_(0.8),
-        planner_("base_local_planner/TrajectoryPlannerROS"),
-        standard_max_vel_(1.0),
-        min_vel_(0.0)
+        dist_err_(0.8)
     {
         while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
         {
@@ -89,19 +87,10 @@
         // suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this);
         // resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this);
         search_server_ = nh.advertiseService("near_wp_nav",&WaypointsNavigation::searchPoseCallback, this);
-        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
+        cmd_vel_sub_ = nh.subscribe("/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
         wp_pub_ = nh.advertise<geometry_msgs::PoseArray>("waypoints", 10);
+        max_vel_pub_ = nh.advertise<std_msgs::Float32>("/max_vel", 1);
         clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
-
-        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_);
     }
 
 
@@ -429,12 +418,12 @@
 
     void startNavigationGL(const geometry_msgs::Pose &dest)
     {
+        setMaxVel();
         move_base_msgs::MoveBaseGoal move_base_goal;
         move_base_goal.target_pose.header.stamp = ros::Time::now();
         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);
     }
 
@@ -449,26 +438,15 @@
 
     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_;
-        }
-        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);
+        float max_vel_rate = waypoint_list_[current_wp_num_].get_vel();
+        std_msgs::Float32 msg;
+        msg.data = max_vel_rate;
+        max_vel_pub_.publish(msg);
     }
 
 
 
+
     void run()
     {
         while(ros::ok()) {
@@ -536,14 +514,14 @@
     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_, planner_;
+    std::string robot_frame_, world_frame_;
     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::Publisher wp_pub_, max_vel_pub_;
     ros::ServiceClient clear_costmaps_srv_;
-    double last_moved_time_, dist_err_, standard_max_vel_, min_vel_;
+    double last_moved_time_, dist_err_;
     unsigned short int current_wp_num_;
 };