diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp index 538ddc7d2948b5b7824c4bbda7c802b8b8363ab1..07256bc697274f52b469213e57d1902713e9f22b 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp @@ -120,6 +120,16 @@ public: */ bool hasSignFlipped(); + /** + * @brief Disable this command trend object if we don't care about this axis + */ + void disable() {enabled_ = false;} + + /** + * @brief Re-enable this command trend object + */ + void enable() {enabled_ = true;} + private: // Simple Enum for Tracking // cppcheck-suppress syntaxError @@ -127,6 +137,7 @@ private: Sign sign_; bool positive_only_, negative_only_; + bool enabled_; }; /** diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index a0b737d910f21c650765bb1ddb83cbf525ffc219..6bf4f672baccff4223f114cc25c7d003899ff803 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -49,6 +49,7 @@ namespace dwb_critics OscillationCritic::CommandTrend::CommandTrend() +: enabled_(true) { reset(); } @@ -62,6 +63,10 @@ void OscillationCritic::CommandTrend::reset() bool OscillationCritic::CommandTrend::update(double velocity) { + if (!enabled_) { + return false; + } + bool flag_set = false; if (velocity < 0.0) { if (sign_ == Sign::POSITIVE) { @@ -81,11 +86,19 @@ bool OscillationCritic::CommandTrend::update(double velocity) bool OscillationCritic::CommandTrend::isOscillating(double velocity) { + if (!enabled_) { + return false; + } + return (positive_only_ && velocity < 0.0) || (negative_only_ && velocity > 0.0); } bool OscillationCritic::CommandTrend::hasSignFlipped() { + if (!enabled_) { + return false; + } + return positive_only_ || negative_only_; } @@ -127,6 +140,13 @@ void OscillationCritic::onInit() // x_only_threshold_ = 0.05; // } + // Disable y oscillation detection if the robot can't move in the Y axis + double max_vel_y; + nh_->get_parameter_or("max_vel_y", max_vel_y, 0.0); + if (max_vel_y == 0.0) { + y_trend_.disable(); + } + reset(); }