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..8a3b5bf4 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 bdb6718a..2f5db414 100644 --- 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 index 8e470cb7..e4dfcfe0 100644 --- 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 index 2614aff2..67a7a8c2 100644 --- 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");