support oscillation angle in move_base

mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h

@@ -134,6 +134,9 @@ class MoveBaseAction
   //! minimal move distance to not detect an oscillation
   double oscillation_distance_;
 
+  //! minimal rotation to not detect an oscillation
+  double oscillation_angle_;
+
   GoalHandle goal_handle_;
 
   std::string name_;

mbf_abstract_nav/src/mbf_abstract_nav/__init__.py

@@ -38,3 +38,5 @@ def add_mbf_abstract_nav_params(gen):
             "How long in seconds to allow for oscillation before executing recovery behaviors", 0.0, 0, 60)
     gen.add("oscillation_distance", double_t, 0,
             "How far in meters the robot must move to be considered not to be oscillating", 0.5, 0, 10)
+    gen.add("oscillation_angle", double_t, 0,
+            "How far in radian the robot must rotate to be considered not to be oscillating", 3.14, 0, 6.28)

mbf_abstract_nav/src/move_base_action.cpp

@@ -57,6 +57,7 @@ MoveBaseAction::MoveBaseAction(const std::string& name, const mbf_utility::Robot
   , action_client_recovery_(private_nh_, "recovery")
   , oscillation_timeout_(0)
   , oscillation_distance_(0)
+  , oscillation_angle_(0)
   , replanning_thread_shutdown_(false)
   , recovery_enabled_(true)
   , behaviors_(behaviors)
@@ -86,6 +87,7 @@ void MoveBaseAction::reconfigure(
     replanning_period_.fromSec(0.0);
   oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
   oscillation_distance_ = config.oscillation_distance;
+  oscillation_angle_ = config.oscillation_angle;
   recovery_enabled_ = config.recovery_enabled;
 }
 
@@ -150,6 +152,7 @@ void MoveBaseAction::start(GoalHandle &goal_handle)
     return;
   }
   goal_pose_ = goal.target_pose;
+  last_oscillation_pose_ = robot_pose_;
 
   // wait for server connections
   if (!action_client_get_path_.waitForServer(connection_timeout) ||
@@ -197,7 +200,8 @@ void MoveBaseAction::actionExePathFeedback(
   {
     // check if oscillating
     // moved more than the minimum oscillation distance
-    if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_)
+    if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_ ||
+        mbf_utility::angle(robot_pose_, last_oscillation_pose_) >= oscillation_angle_)
     {
       last_oscillation_reset_ = ros::Time::now();
       last_oscillation_pose_ = robot_pose_;

mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp

@@ -70,6 +70,7 @@ mbf_abstract_nav::MoveBaseFlexConfig CostmapControllerExecution::toAbstract(cons
   abstract_config.controller_max_retries = config.controller_max_retries;
   abstract_config.oscillation_timeout = config.oscillation_timeout;
   abstract_config.oscillation_distance = config.oscillation_distance;
+  abstract_config.oscillation_angle = config.oscillation_angle;
   return abstract_config;
 }
 

mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp

@@ -463,6 +463,7 @@ void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig& c
   abstract_config.recovery_patience = config.recovery_patience;
   abstract_config.oscillation_timeout = config.oscillation_timeout;
   abstract_config.oscillation_distance = config.oscillation_distance;
+  abstract_config.oscillation_angle = config.oscillation_angle;
   abstract_config.restore_defaults = config.restore_defaults;
   mbf_abstract_nav::AbstractNavigationServer::reconfigure(abstract_config, level);