Code cleaning

rotational_recovery/src/rotational_recovery.cpp

@@ -149,7 +149,7 @@ namespace rotational_recovery
     cost = normalizedPoseCost(current);
     double t; // Will hold the first time that is invalid
     geometry_msgs::Pose2D current_tmp = current;
-    ROS_INFO_NAMED ("top", "init = (%.2f, %.2f, %.2f), current = (%.2f, %.2f, %.2f)",
+    ROS_DEBUG_NAMED ("top", "init = (%.2f, %.2f, %.2f), current = (%.2f, %.2f, %.2f)",
                     current.x, current.y, current.theta, current_tmp.x, current_tmp.y, current_tmp.theta);
     double next_cost;
 

twist_recovery/src/twist_recovery.cpp

@@ -194,6 +194,7 @@ namespace twist_recovery
   void TwistRecovery::runBehavior ()
   {
     ROS_ASSERT (initialized_);
+    double stop_time = 0.5;
 
     // Threre is a maximum duration this recovery can run defined in duration_.
     // However, there may be an obstacle so it is necessary to figure out how
@@ -210,6 +211,13 @@ namespace twist_recovery
       pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, max_duration-t));
       r.sleep();
     }
+
+    base_frame_twist_.linear.x = 0.0;
+    ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time);
+    for (double t=0; t<stop_time; t+=1/controller_frequency_) {
+      pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, stop_time-t));
+      r.sleep();
+    }
   }
 
 } // namespace twist_recovery