From 4bc3e8a534005e983ee7a49a09ec00aeb9820661 Mon Sep 17 00:00:00 2001 From: Max Kirchhoff <151524065+IPA-MKI@users.noreply.github.com> Date: Wed, 22 Nov 2023 17:42:09 +0100 Subject: [PATCH] Move isValidVelocity back to kinematic_parameters (#3973) * fix(xy_theta_iterator): isValidSpeed should check function params only * feat(kinematic_params): move isValidSpeed() back to kinematic_params as it was in ROS1. --- .../dwb_plugins/kinematic_parameters.hpp | 37 +++++++++++++++++-- .../include/dwb_plugins/xy_theta_iterator.hpp | 12 ------ .../dwb_plugins/src/xy_theta_iterator.cpp | 25 +------------ 3 files changed, 36 insertions(+), 38 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index ed3cabc3d00..bfed0c2ec54 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -35,12 +35,13 @@ #ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ #define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_ +#include #include #include #include -#include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" namespace dwb_plugins { @@ -75,6 +76,36 @@ struct KinematicParameters inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;} inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;} + /** + * @brief Check to see whether the combined x/y/theta velocities are valid + * @return True if the magnitude hypot(x,y) and theta are within the robot's + * absolute limits + * + * This is based on three parameters: min_speed_xy, max_speed_xy and + * min_speed_theta. The speed is valid if 1) The combined magnitude hypot(x,y) + * is less than max_speed_xy (or max_speed_xy is negative) AND 2) min_speed_xy + * is negative or min_speed_theta is negative or hypot(x,y) is greater than + * min_speed_xy or fabs(theta) is greater than min_speed_theta. + */ + inline bool isValidSpeed(double x, double y, double theta) + { + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && + vmag_sq > max_speed_xy_sq_ + std::numeric_limits::epsilon()) + { + return false; + } + if ( + min_speed_xy_ >= 0.0 && vmag_sq + std::numeric_limits::epsilon() < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && + fabs(theta) + std::numeric_limits::epsilon() < min_speed_theta_) + { + return false; + } + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; + } + protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -127,8 +158,8 @@ class KinematicsHandler * @brief Callback executed when a paramter change is detected * @param parameters list of changed parameters */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 88c18046ed5..68ac9405c2f 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -58,18 +58,6 @@ class XYThetaIterator : public VelocityIterator nav_2d_msgs::msg::Twist2D nextTwist() override; protected: - /** - * @brief Check to see whether the combined x/y/theta velocities are valid - * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits - * - * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. - * The speed is valid if - * 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) - * AND - * 2) min_speed_xy is negative or min_speed_theta is negative or - * hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta. - */ - bool isValidSpeed(double x, double y, double theta); virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 50b19aa337c..5ca1992c6bc 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -41,8 +41,6 @@ #include "nav_2d_utils/parameters.hpp" #include "nav2_util/node_utils.hpp" -#define EPSILON 1E-5 - namespace dwb_plugins { void XYThetaIterator::initialize( @@ -92,29 +90,10 @@ void XYThetaIterator::startNewIteration( } } -bool XYThetaIterator::isValidSpeed(double x, double y, double theta) -{ - KinematicParameters kinematics = kinematics_handler_->getKinematics(); - double vmag_sq = x * x + y * y; - if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) { - return false; - } - if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() && - kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta()) - { - return false; - } - if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) { - return false; - } - return true; -} - bool XYThetaIterator::isValidVelocity() { - return isValidSpeed( - x_it_->getVelocity(), y_it_->getVelocity(), - th_it_->getVelocity()); + return kinematics_handler_->getKinematics().isValidSpeed( + x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } bool XYThetaIterator::hasMoreTwists()