diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index a337f0f5772a2174ef607393bdad31300f53449b..0266ffa70638dc90ec53f1c6a198340304ce6db6 100644 --- a/rotational_recovery/src/rotational_recovery.cpp +++ b/rotational_recovery/src/rotational_recovery.cpp @@ -263,7 +263,7 @@ namespace rotational_recovery void RotationalRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration) { ros::Rate r(controller_frequency_); - ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, + ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, twist.linear.y, twist.angular.z, max_duration); // We will now apply this twist open-loop for d seconds (scaled so we can @@ -278,6 +278,7 @@ namespace rotational_recovery // Apply algorithm to escape from a lethal obstacle void RotationalRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost) { + ROS_WARN_NAMED ("top", "RotationalRecovery: Applying escape from lethal cost algorithm"); double cost_front_1, cost_front_2, cost_front_3 = 0; double cost_back_1, cost_back_2, cost_back_3 = 0; double cost_left_1, cost_left_2, cost_left_3 = 0; @@ -590,7 +591,7 @@ namespace rotational_recovery // We apply this twist open-loop for d seconds (scaled so we can guarantee // stopping at the end) - ROS_INFO_NAMED ("top", "Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z); + ROS_INFO_NAMED ("top", "TwistRecovery: Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z); for (double t=0; t<max_duration; t+=1/controller_frequency_) { if(t<0.05){ ROS_DEBUG_NAMED ("top", "twist.linear.x=%.2f, .y=%.2f, .z=%.2f, .angular.x=%.2f, .y=%.2f, .z=%.2f", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z); @@ -602,7 +603,7 @@ namespace rotational_recovery } twist.angular.z = 0.0; - ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time); + ROS_INFO_NAMED ("top", "TwistRecovery: Applying STOP for %.2f s", stop_time); for (double t=0; t<stop_time; t+=1/controller_frequency_) { pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t)); r.sleep(); diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp index 6fde4205e3b1a902a1b71f5fde9cb8133cfc28b3..3a8009afac4fc5d0040bca126ba440d7593a7ecd 100644 --- a/twist_recovery/src/twist_recovery.cpp +++ b/twist_recovery/src/twist_recovery.cpp @@ -209,7 +209,7 @@ namespace twist_recovery void TwistRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration) { ros::Rate r(controller_frequency_); - ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, + ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, twist.linear.y, twist.angular.z, max_duration); // We will now apply this twist open-loop for d seconds (scaled so we can @@ -224,6 +224,7 @@ namespace twist_recovery // Apply algorithm to escape from a lethal obstacle void TwistRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost) { + ROS_WARN_NAMED ("top", "TwistRecovery: Applying escape from lethal cost algorithm"); double cost_front_1, cost_front_2, cost_front_3 = 0; double cost_back_1, cost_back_2, cost_back_3 = 0; double cost_left_1, cost_left_2, cost_left_3 = 0; @@ -526,7 +527,7 @@ namespace twist_recovery else { const double max_duration = nonincreasingCostInterval(current_pose, base_frame_twist_); twist.linear.x = -0.2; - ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, + ROS_WARN_NAMED ("top", "TwistRecovery: Applying twist (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x, twist.linear.y, twist.angular.z, max_duration); // We will now apply this twist open-loop for d seconds (scaled so we can @@ -539,7 +540,7 @@ namespace twist_recovery } twist.linear.x = 0.0; - ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time); + ROS_INFO_NAMED ("top", "TwistRecovery: Applying STOP for %.2f s", stop_time); for (double t=0; t<stop_time; t+=1/controller_frequency_) { pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t)); r.sleep();