ControllerAction: use dynamic params on oscillation detection (as Mov…

…eBaseAction does)
naturerobots · Nov 24, 2023 · 050ac09 · 050ac09
1 parent 60d3333
commit 050ac09
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 20 deletions.
diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp
@@ -198,8 +198,7 @@ class AbstractActionBase
     slot.in_use = false;
   }
 
-  virtual void reconfigureAll(
-      mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
+  virtual void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
   {
     boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
 

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h
@@ -106,7 +106,7 @@ class AbstractExecutionBase
    virtual void postRun(){};
 
    /**
-    * @brief Optional implementaiton-specific configuration function.
+    * @brief Optional implementation-specific configuration function.
     */
    virtual void reconfigure(MoveBaseFlexConfig& _cfg)
    {

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
@@ -62,6 +62,8 @@ class ControllerAction :
   ControllerAction(const std::string &name,
                    const mbf_utility::RobotInformation &robot_info);
 
+  void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig& config, uint32_t level) override;
+
   /**
    * @brief Start controller action.
    * Override abstract action version to allow updating current plan without stopping execution.
@@ -95,6 +97,14 @@ class ControllerAction :
   geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
   geometry_msgs::PoseStamped goal_pose_;  ///< Current goal pose
 
+  //! timeout after a oscillation is detected
+  ros::Duration oscillation_timeout_;
+
+  //! minimal move distance to not detect an oscillation
+  double oscillation_distance_;
+
+  //! minimal rotation to not detect an oscillation
+  double oscillation_angle_;
 };
 }
 

diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp
@@ -367,9 +367,9 @@ void AbstractNavigationServer::reconfigure(
     // if someone sets restore defaults on the parameter server, prevent looping
     config.restore_defaults = false;
   }
-  planner_action_.reconfigureAll(config, level);
-  controller_action_.reconfigureAll(config, level);
-  recovery_action_.reconfigureAll(config, level);
+  planner_action_.reconfigure(config, level);
+  controller_action_.reconfigure(config, level);
+  recovery_action_.reconfigure(config, level);
   move_base_action_.reconfigure(config, level);
 
   last_config_ = config;

diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
@@ -50,6 +50,15 @@ ControllerAction::ControllerAction(
 {
 }
 
+void ControllerAction::reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
+{
+  AbstractActionBase::reconfigure(config, level);
+
+  oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
+  oscillation_distance_ = config.oscillation_distance;
+  oscillation_angle_ = config.oscillation_angle;
+}
+
 void ControllerAction::start(
     GoalHandle &goal_handle,
     typename AbstractControllerExecution::Ptr execution_ptr
@@ -115,16 +124,6 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
 
   ros::NodeHandle private_nh("~");
 
-  double oscillation_timeout_tmp;
-  private_nh.param("oscillation_timeout", oscillation_timeout_tmp, 0.0);
-  ros::Duration oscillation_timeout(oscillation_timeout_tmp);
-
-  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,16 +275,16 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
         break;
 
       case AbstractControllerExecution::GOT_LOCAL_CMD:
-        if (!oscillation_timeout.isZero())
+        if (!oscillation_timeout_.isZero())
         {
           // check if oscillating
-          if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance ||
-              mbf_utility::angle(robot_pose_, oscillation_pose) >= oscillation_angle)
+          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_;
           }
-          else if (last_oscillation_reset + oscillation_timeout < ros::Time::now())
+          else if (last_oscillation_reset + oscillation_timeout_ < ros::Time::now())
           {
             ROS_WARN_STREAM_NAMED(name_, "The controller is oscillating for "
                 << (ros::Time::now() - last_oscillation_reset).toSec() << "s");