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

…eBaseAction does)
naturerobots · Dec 21, 2023 · ee5eda2 · ee5eda2
1 parent 3c7b7f1
commit ee5eda2
Show file tree

Hide file tree

Showing 5 changed files with 29 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
@@ -59,6 +59,8 @@ class ControllerAction : public AbstractActionBase<mbf_msgs::ExePathAction, Abst
 
   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.
@@ -87,6 +89,15 @@ class ControllerAction : public AbstractActionBase<mbf_msgs::ExePathAction, Abst
 
   //! Publish the current goal pose (the last pose of the path we are following)
   ros::Publisher goal_pub_;
+
+  //! 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_;
 };
 }  // namespace mbf_abstract_nav
 

diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp
@@ -363,9 +363,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
@@ -53,6 +53,15 @@ ControllerAction::ControllerAction(
   goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("controller_goal", 1);
 }
 
+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
@@ -119,16 +128,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;
 
@@ -281,16 +280,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");