From 09192fa92177e36e75d13cdd4ec49be5dde62148 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?= <jdominguez@iri.upc.edu> Date: Wed, 27 Nov 2019 13:00:53 +0100 Subject: [PATCH] Code cleaning --- rotational_recovery/src/rotational_recovery.cpp | 2 +- twist_recovery/src/twist_recovery.cpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index a7010ad..40e931f 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 bb110c3..d2839f4 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 -- GitLab