Add recovery time output (#54)

* Add recovery time output * Fix bug related to count --------- Co-authored-by: amslrobots <amslrobots@gmail.com>
amslabtech · Aug 18, 2024 · d572ff9 · d572ff9
1 parent 95ff5d4
commit d572ff9
Showing 1 changed file with 6 additions and 1 deletion.
diff --git 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<float>(hz_));
       recovery_params_.recovery_count++;
     }
     else if (is_stuck())
     {
+      ROS_WARN_STREAM_THROTTLE(0.5, "stuck time: " << recovery_params_.stuck_count / static_cast<float>(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);
       }
     }