publisher in mainNodeThread since cmd_vel is not sent in a constant rate

include/safe_cmd_alg_node.h

@@ -46,6 +46,7 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
     // [publisher attributes]
     ros::Publisher cmd_vel_safe_publisher_;
     geometry_msgs::Twist Twist_msg_;
+    geometry_msgs::Twist last_twist_;
 
     // [subscriber attributes]
     ros::Subscriber cmd_vel_subscriber_;

src/safe_cmd_alg_node.cpp

@@ -35,12 +35,24 @@ void SafeCmdAlgNode::mainNodeThread(void)
 {
   // [fill msg structures]
   //this->Twist_msg_.data = my_var;
-  
+  Twist_msg_ = last_twist_;
+  if(front_obstacle_ && last_twist_.linear.x > 0.0)
+  {
+    Twist_msg_.linear.x = 0.0;
+    ROS_WARN("heading to front obstacle avoided");
+  }  
+
+  if(rear_obstacle_ && last_twist_.linear.x < 0.0)
+  {
+    Twist_msg_.linear.x = 0.0; 
+    ROS_WARN("heading to rear obstacle avoided");
+  }  
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  cmd_vel_safe_publisher_.publish(Twist_msg_);
   
 }
 
@@ -52,16 +64,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
   //use appropiate mutex to shared variables if necessary 
   //this->alg_.lock(); 
   //this->cmd_vel_mutex_.enter(); 
-  Twist_msg_=*msg;
-
-  if(front_obstacle_ && msg->linear.x > 0.0)
-    Twist_msg_.linear.x = 0.0;
-
-  if(rear_obstacle_ && msg->linear.x < 0.0)
-    Twist_msg_.linear.x = 0.0;
-
-  cmd_vel_safe_publisher_.publish(Twist_msg_);
-
+  last_twist_=*msg;
 
   //unlock previously blocked shared variables 
   //this->alg_.unlock();