diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index f3cdc1abc206604c5656cac613129cd0d09fbd9a..407e125a757bd1c58759d83a6f31eecfcf9054b4 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,10 +4,10 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
   collision_time_(1),
   min_dist_(0.4),
-  max_vel_front_(1.0),
-  max_vel_rear_(1.0),
-  limit_vel_front_(1.0),
-  limit_vel_rear_(1.0),
+  max_vel_front_(0.0),
+  max_vel_rear_(0.0),
+  limit_vel_front_(0.0),
+  limit_vel_rear_(0.0),
   front_laser_received_(false),
   rear_laser_received_(false),
   new_cmd_vel(false)
@@ -40,8 +40,6 @@ SafeCmdAlgNode::~SafeCmdAlgNode(void)
 void SafeCmdAlgNode::mainNodeThread(void)
 {
   this->alg_.lock();
-  // [fill msg structures]
-  //Twist_msg_.data = my_var;
   
   if(!alg_.config_.unsafe)
   {
@@ -49,27 +47,19 @@ void SafeCmdAlgNode::mainNodeThread(void)
     this->update_rear_laser_watchdog();
     if(this->front_laser_watchdog_active())
     {
-      ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+      ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
       Twist_msg_.linear.x = 0.0;
     }
     else if(this->rear_laser_watchdog_active())
     {
-      ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+      ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
       Twist_msg_.linear.x = 0.0;
     }
     else
     {
-      // if(!front_laser_received_)
-      //   ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser not received");
-      // if(!rear_laser_received_)
-      //   ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser not received");
-      // 
-      // front_laser_received_ = false;
-      // rear_laser_received_  = false;
-
       if(Twist_msg_.linear.x > fabs(max_vel_front_))
       {
-        ROS_WARN_STREAM("heading to Front obstacle, reducing velocity"<<fabs(max_vel_front_));
+        ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing forward velocity from "<< Twist_msg_.linear.x << " to "<<fabs(max_vel_front_));
         Twist_msg_.linear.x = fabs(max_vel_front_);
         if(max_vel_front_==0)
           Twist_msg_.angular.z = 0;
@@ -77,93 +67,73 @@ void SafeCmdAlgNode::mainNodeThread(void)
 
       if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
       {
-        ROS_WARN_STREAM("heading to Rear obstacle, reducing velocity"<<fabs(max_vel_rear_));
+        ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing backward velocity from "<< Twist_msg_.linear.x << " to "<<-fabs(max_vel_rear_));
         Twist_msg_.linear.x = -fabs(max_vel_rear_);
         if(max_vel_rear_==0)
           Twist_msg_.angular.z = 0;
       }
     }
   }
-  // [fill srv structure and make request to the server]
-  
-  // [fill action structure and make request to the action server]
 
-  // [publish messages]
   if(this->new_cmd_vel)
   {
     cmd_vel_safe_publisher_.publish(Twist_msg_);
     this->new_cmd_vel=false;
   }
+
+  // [fill msg structures]
+
+  // [fill srv structure and make request to the server]
   
-  this->alg_.unlock();
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
   
+  this->alg_.unlock();
 }
 
 /*  [subscriber callbacks] */
 void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received"); 
-
-  //use appropiate mutex to shared variables if necessary 
   this->alg_.lock(); 
   //cmd_vel_mutex_.enter();
 
-  if(!alg_.config_.unsafe)
-  {
-    if (msg->linear.x == 0 && msg->angular.z ==0)
-      Twist_msg_ = *msg;
-    else
-    {
-      Twist_msg_.linear.x  += (msg->linear.x  - last_twist_.linear.x ); 
-      Twist_msg_.linear.y  += (msg->linear.y  - last_twist_.linear.y ); 
-      Twist_msg_.linear.z  += (msg->linear.z  - last_twist_.linear.z ); 
-      Twist_msg_.angular.x += (msg->angular.x - last_twist_.angular.x); 
-      Twist_msg_.angular.y += (msg->angular.y - last_twist_.angular.y); 
-      Twist_msg_.angular.z += (msg->angular.z - last_twist_.angular.z); 
-    }
-    last_twist_= *msg;
-  } else
-    Twist_msg_ = *msg;
+  Twist_msg_=*msg;
   this->new_cmd_vel=true;
-  //unlock previously blocked shared variables 
-  this->alg_.unlock(); 
+
   //cmd_vel_mutex_.exit(); 
+  this->alg_.unlock(); 
 }
 void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received"); 
-
-  //use appropiate mutex to shared variables if necessary 
   this->alg_.lock(); 
   //rear_laser_mutex_.enter(); 
-  
-  this->reset_rear_laser_watchdog();
 
+  this->reset_rear_laser_watchdog();
   max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
   rear_laser_received_ = true;
   //ROS_INFO("Max vel r: %f",max_vel_rear_);
 
-  //unlock previously blocked shared variables 
-  this->alg_.unlock(); 
   //rear_laser_mutex_.exit(); 
+  this->alg_.unlock(); 
+
 }
 void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received"); 
-
-  //use appropiate mutex to shared variables if necessary 
   this->alg_.lock(); 
   //front_laser_mutex_.enter();
-  
-  this->reset_front_laser_watchdog();
 
+  this->reset_front_laser_watchdog();
   max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
   front_laser_received_ = true;
   //ROS_INFO("Max vel f: %f",max_vel_front_);
 
-  //unlock previously blocked shared variables 
-  this->alg_.unlock(); 
   //front_laser_mutex_.exit(); 
+  this->alg_.unlock(); 
+  
 }
 
 /*  [service callbacks] */
@@ -175,11 +145,13 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
 void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
+
   collision_time_  = config.collision_time;
   min_dist_        = config.min_dist;
   limit_vel_front_ = config.limit_vel_front;
   limit_vel_rear_  = config.limit_vel_rear;
   this->config=config;
+
   this->alg_.unlock();
 }
 
@@ -206,13 +178,15 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP
   float min_range    = *std::min_element( scan->ranges.begin(), scan->ranges.end(),min_test_);
   int   min_pos      = distance(scan->ranges.begin(),std::min_element( scan->ranges.begin(), scan->ranges.end() ));
   float max_velocity = 0;
-
-  ROS_DEBUG_STREAM("compute_max_velocity frame: " << scan->header.frame_id << 
-                   " min range: " << min_range << " at " << min_pos << " of " << scan->ranges.size());
   
+  //float x = scan->ranges[min_pos]*cos(scan->angle_min + min_pos*scan->angle_increment);
+  //float y = scan->ranges[min_pos]*sin(scan->angle_min + min_pos*scan->angle_increment);
+
   if (min_range >= min_dist_)
     max_velocity = min_range / collision_time_;
 
+  //ROS_INFO_STREAM("SafeCmdAlgNode::compute_max_velocity_: min_range=" << min_range << ", max_velocity=" << max_velocity);
+
   return max_velocity;
 }