From d572ff98f68707b3e1130cf1578856fbee81c05d Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura <82020865+ToshikiNakamura0412@users.noreply.github.com> Date: Sun, 18 Aug 2024 19:05:08 +0900 Subject: [PATCH] Add recovery time output (#54) * Add recovery time output * Fix bug related to count --------- Co-authored-by: amslrobots --- src/point_follow_planner.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp index 7c4ff44..fbbfd7d 100644 --- a/src/point_follow_planner.cpp +++ b/src/point_follow_planner.cpp @@ -558,16 +558,21 @@ geometry_msgs::Twist PointFollowPlanner::calc_cmd_vel() { if (0 < recovery_params_.recovery_count && recovery_params_.recovery_count <= recovery_params_.max_recovery_count) { - ROS_WARN_THROTTLE(1.0, "Recovery mode is activated"); + ROS_WARN_THROTTLE(0.5, "################"); + ROS_WARN_THROTTLE(0.5, "### recovery ###"); + ROS_WARN_THROTTLE(0.5, "################"); + ROS_WARN_STREAM_THROTTLE(0.5, "recovery time: " << recovery_params_.recovery_count / static_cast(hz_)); recovery_params_.recovery_count++; } else if (is_stuck()) { + ROS_WARN_STREAM_THROTTLE(0.5, "stuck time: " << recovery_params_.stuck_count / static_cast(hz_)); recovery_params_.recovery_count = 0; recovery_params_.stuck_count++; if (recovery_params_.stuck_count_th <= recovery_params_.stuck_count) { recovery_params_.recovery_count++; + recovery_params_.stuck_count = 0; sound(recovery_params_.sound_file); } }