diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 5c88505a3b624cac663cef9695df3cbecd4338a1..d8ddaaf910ae1708910f45d74a369ba767f63767 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -63,6 +63,7 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
 
     bool front_laser_received_;
     bool rear_laser_received_;
+    bool new_cmd_vel;
 
     // [service attributes]
 
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index bb62b2adb35048173ca1753ff34261bedca6757e..f3cdc1abc206604c5656cac613129cd0d09fbd9a 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -9,7 +9,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   limit_vel_front_(1.0),
   limit_vel_rear_(1.0),
   front_laser_received_(false),
-  rear_laser_received_(false)
+  rear_laser_received_(false),
+  new_cmd_vel(false)
 {
   //init class attributes if necessary
   loop_rate_ = 20;//in [Hz]
@@ -88,7 +89,11 @@ void SafeCmdAlgNode::mainNodeThread(void)
   // [fill action structure and make request to the action server]
 
   // [publish messages]
-  cmd_vel_safe_publisher_.publish(Twist_msg_);
+  if(this->new_cmd_vel)
+  {
+    cmd_vel_safe_publisher_.publish(Twist_msg_);
+    this->new_cmd_vel=false;
+  }
   
   this->alg_.unlock();
   
@@ -119,7 +124,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
     last_twist_= *msg;
   } else
     Twist_msg_ = *msg;
-
+  this->new_cmd_vel=true;
   //unlock previously blocked shared variables 
   this->alg_.unlock(); 
   //cmd_vel_mutex_.exit();