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 index f5bdbb8b..3b2e2cde 100644 --- 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 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 index 28cc5b75..9ad30605 100644 --- 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 index 3d8ca7ac..bc16717c 100644 --- 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("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");