Move isValidVelocity back to kinematic_parameters (ros-navigation#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.

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 <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
-#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<float>::epsilon())
+    {
+      return false;
+    }
+    if (
+      min_speed_xy_ >= 0.0 && vmag_sq + std::numeric_limits<float>::epsilon() < min_speed_xy_sq_ &&
+      min_speed_theta_ >= 0.0 &&
+      fabs(theta) + std::numeric_limits<float>::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<rclcpp::Parameter> parameters);
+  rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
+    std::vector<rclcpp::Parameter> parameters);
   void update_kinematics(KinematicParameters kinematics);
   std::string plugin_name_;
 };

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_;

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()