iri_safe_cmd: publish cmd_vel messages ONLY if new input cmd_vel msg received

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]
 

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();