diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index a7010ad14aedacba671a3a67a710b98ad7317ea5..40e931f77d712f7fc318afd691ae743dfc2c4499 100644 --- a/rotational_recovery/src/rotational_recovery.cpp +++ b/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; diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp index bb110c35bfeb88cf33247dfe719b2587ba0b64a3..d2839f4bb7fdd79fa327bc769feaa11d96beaa1c 100644 --- a/twist_recovery/src/twist_recovery.cpp +++ b/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