fix angle oscillation

naturerobots · Nov 7, 2023 · c02460e · c02460e
1 parent 3dedf54
commit c02460e
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
@@ -122,6 +122,9 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
   double oscillation_distance;
   private_nh.param("oscillation_distance", oscillation_distance, 0.03);
 
+  double oscillation_angle;
+  private_nh.param("oscillation_angle", oscillation_angle, M_PI);
+
   mbf_msgs::ExePathResult result;
   mbf_msgs::ExePathFeedback feedback;
 
@@ -276,7 +279,8 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         if (!oscillation_timeout.isZero())
         {
           // check if oscillating
-          if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance)
+          if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance ||
+              mbf_utility::angle(robot_pose_, oscillation_pose) >= oscillation_angle)
           {
             last_oscillation_reset = ros::Time::now();
             oscillation_pose = robot_pose_;