diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 2caec97ff3..96ca331e37 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -61,11 +61,26 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above. +## Dynamic Window Pure Pursuit Features + +This controller also implements the Dynamic Window Pure Pursuit (DWPP) algorithm, developed by [Fumiya Ohnishi](https://www.linkedin.com/in/fumiya-ohnishi-23b124202). +Unlike the standard Pure Pursuit, DWPP enables the consideration of velocity and acceleration constraints when computing velocity commands. +An overview of the algorithm can be found here: [DWPP Algorithm](https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller/blob/main/algorithm.md). + +A link to the paper and its citation will be provided once it becomes publicly available. + ## Configuration | Parameter | Description | |-----|----| -| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_vel` | The maximum linear velocity to use. | +| `max_angular_accel` | The maximum angular acceleration to use. | +| `min_linear_vel` | The minimum linear velocity used when `use_dynamic_window` is `true`. | +| `max_angular_vel` | The maximum angular velocity used when `use_dynamic_window` is `true`. | +| `min_angular_vel` | The minimum angular velocity used when `use_dynamic_window` is `true`. | +| `max_linear_accel` | The maximum linear acceleration used when `use_dynamic_window` is `true`. | +| `max_linear_decel` | The maximum linear deceleration used when `use_dynamic_window` is `true`. | +| `max_angular_decel` | The maximum angular deceleration used when `use_dynamic_window` is `true`. | | `lookahead_dist` | The lookahead distance to use to find the lookahead point | | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | @@ -88,10 +103,10 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. | | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | -| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | +| `use_dynamic_window` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. | Example fully-described XML with default parameter values: @@ -117,7 +132,14 @@ controller_server: stateful: True FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.5 + max_linear_vel: 0.5 + min_linear_vel: 0.0 + max_angular_vel: 1.0 + min_angular_vel: -1.0 + max_linear_accel: 0.5 + max_linear_decel: 0.5 + max_angular_accel: 1.0 + max_angular_decel: 1.0 lookahead_dist: 0.6 min_lookahead_dist: 0.3 max_lookahead_dist: 0.9 @@ -126,7 +148,7 @@ controller_server: transform_tolerance: 0.1 use_velocity_scaled_lookahead_dist: false min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 1.0 + approach_velocity_scaling_dist: 0.6 use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true @@ -134,15 +156,17 @@ controller_server: regulated_linear_scaling_min_radius: 0.9 regulated_linear_scaling_min_speed: 0.25 use_fixed_curvature_lookahead: false - curvature_lookahead_dist: 1.0 + curvature_lookahead_dist: 0.6 use_rotate_to_heading: true + allow_reversing: false rotate_to_heading_min_angle: 0.785 - max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 interpolate_curvature_after_goal: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 + min_distance_to_obstacle: 0.0 + use_dynamic_window: false ``` ## Topics @@ -165,3 +189,7 @@ To tune to get Pure Pursuit behaviors, set all boolean parameters to false and m Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. + +When `use_dynamic_window` is set to True, the velocity, acceleration, and deceleration limits are enforced during the velocity command computation. +Note that the velocity smoother clips the velocity commands output by this controller based on its own velocity and acceleration constraints before publishing cmd_vel. +Therefore, the velocity smoother’s `max_velocity`, `min_velocity`, `max_accel`, and `max_decel` parameters must be consistent with this controller’s corresponding velocity, acceleration, and deceleration settings. diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp new file mode 100644 index 0000000000..9677be534c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp @@ -0,0 +1,286 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +namespace dynamic_window_pure_pursuit +{ + +struct DynamicWindowBounds +{ + double max_linear_vel; + double min_linear_vel; + double max_angular_vel; + double min_angular_vel; +}; + +/** + * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints. + * @param current_speed Current linear and angular velocity of the robot + * @param max_linear_vel Maximum allowable linear velocity + * @param min_linear_vel Minimum allowable linear velocity + * @param max_angular_vel Maximum allowable angular velocity + * @param min_angular_vel Minimum allowable angular velocity + * @param max_linear_accel Maximum allowable linear acceleration + * @param max_linear_decel Maximum allowable linear deceleration + * @param max_angular_accel Maximum allowable angular acceleration + * @param max_angular_decel Maximum allowable angular deceleration + * @param dt Control duration + * @return Computed dynamic window's velocity bounds + */ +inline DynamicWindowBounds computeDynamicWindow( + const geometry_msgs::msg::Twist & current_speed, + const double & max_linear_vel, + const double & min_linear_vel, + const double & max_angular_vel, + const double & min_angular_vel, + const double & max_linear_accel, + const double & max_linear_decel, + const double & max_angular_accel, + const double & max_angular_decel, + const double & dt +) +{ + DynamicWindowBounds dynamic_window; + constexpr double Eps = 1e-3; + + // function to compute dynamic window for a single dimension + auto compute_window = [&](const double & current_vel, const double & max_vel, + const double & min_vel, const double & max_accel, const double & max_decel) + { + double candidate_max_vel = 0.0; + double candidate_min_vel = 0.0; + + if (current_vel > Eps) { + // if the current velocity is positive, acceleration means an increase in speed + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_decel * dt; + } else if (current_vel < -Eps) { + // if the current velocity is negative, acceleration means a decrease in speed + candidate_max_vel = current_vel + max_decel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } else { + // if the current velocity is zero, allow acceleration in both directions. + candidate_max_vel = current_vel + max_accel * dt; + candidate_min_vel = current_vel - max_accel * dt; + } + + // clip to max/min velocity limits + double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel); + double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel); + return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel); + }; + + // linear velocity + std::tie(dynamic_window.max_linear_vel, + dynamic_window.min_linear_vel) = compute_window(current_speed.linear.x, + max_linear_vel, min_linear_vel, + max_linear_accel, max_linear_decel); + + // angular velocity + std::tie(dynamic_window.max_angular_vel, + dynamic_window.min_angular_vel) = compute_window(current_speed.angular.z, + max_angular_vel, min_angular_vel, + max_angular_accel, max_angular_decel); + + return dynamic_window; +} + +/** + * @brief Apply regulated linear velocity to the dynamic window + * @param regulated_linear_vel Regulated linear velocity + * @param dynamic_window Dynamic window to be regulated + */ +inline void applyRegulationToDynamicWindow( + const double & regulated_linear_vel, + DynamicWindowBounds & dynamic_window) +{ + double regulated_dynamic_window_max_linear_vel; + double regulated_dynamic_window_min_linear_vel; + + // Extract the portion of the dynamic window that lies within the range [0, regulated_linear_vel] + if (regulated_linear_vel >= 0.0) { + regulated_dynamic_window_max_linear_vel = std::min( + dynamic_window.max_linear_vel, regulated_linear_vel); + regulated_dynamic_window_min_linear_vel = std::max( + dynamic_window.min_linear_vel, 0.0); + } else { + regulated_dynamic_window_max_linear_vel = std::min( + dynamic_window.max_linear_vel, 0.0); + regulated_dynamic_window_min_linear_vel = std::max( + dynamic_window.min_linear_vel, regulated_linear_vel); + } + + if (regulated_dynamic_window_max_linear_vel < regulated_dynamic_window_min_linear_vel) { + // No valid portion of the dynamic window remains after applying the regulation + if (regulated_dynamic_window_min_linear_vel > 0.0) { + // If the dynamic window is entirely in the positive range, + // collapse both bounds to dynamic_window_min_linear_vel + regulated_dynamic_window_max_linear_vel = regulated_dynamic_window_min_linear_vel; + } else { + // If the dynamic window is entirely in the negative range, + // collapse both bounds to dynamic_window_max_linear_vel + regulated_dynamic_window_min_linear_vel = regulated_dynamic_window_max_linear_vel; + } + } + + dynamic_window.max_linear_vel = regulated_dynamic_window_max_linear_vel; + dynamic_window.min_linear_vel = regulated_dynamic_window_min_linear_vel; +} + + +/** + * @brief Compute the optimal velocity to follow the path within the dynamic window + * @param dynamic_window Dynamic window defining feasible velocity bounds + * @param curvature Curvature of the path to follow + * @param sign Velocity sign (forward or backward) + * @return Optimal linear and angular velocity + */ +inline std::tuple computeOptimalVelocityWithinDynamicWindow( + const DynamicWindowBounds & dynamic_window, + const double & curvature, + const double & sign +) +{ + double optimal_linear_vel; + double optimal_angular_vel; + + // consider linear_vel - angular_vel space (horizontal and vertical axes respectively) + // Select the closest point to the line + // angular_vel = curvature * linear_vel within the dynamic window. + // If multiple points are equally close, select the one with the largest linear_vel. + + // When curvature == 0, the line is angular_vel = 0 + if (abs(curvature) < 1e-3) { + // linear velocity + if (sign >= 0.0) { + // If moving forward, select the max linear vel + optimal_linear_vel = dynamic_window.max_linear_vel; + } else { + // If moving backward, select the min linear vel + optimal_linear_vel = dynamic_window.min_linear_vel; + } + + // angular velocity + // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0 + if (dynamic_window.min_angular_vel <= 0.0 && 0.0 <= dynamic_window.max_angular_vel) { + optimal_angular_vel = 0.0; + } else { + // If not, select angular vel within dynamic window closest to 0 + if (std::abs(dynamic_window.min_angular_vel) <= std::abs(dynamic_window.max_angular_vel)) { + optimal_angular_vel = dynamic_window.min_angular_vel; + } else { + optimal_angular_vel = dynamic_window.max_angular_vel; + } + } + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel intersect, + // select the intersection point that yields the highest linear velocity. + + // List the four candidate intersection points + std::pair candidates[] = { + {dynamic_window.min_linear_vel, curvature * dynamic_window.min_linear_vel}, + {dynamic_window.max_linear_vel, curvature * dynamic_window.max_linear_vel}, + {dynamic_window.min_angular_vel / curvature, dynamic_window.min_angular_vel}, + {dynamic_window.max_angular_vel / curvature, dynamic_window.max_angular_vel} + }; + + double best_linear_vel = -std::numeric_limits::infinity() * sign; + double best_angular_vel = 0.0; + + for (auto [linear_vel, angular_vel] : candidates) { + // Check whether the candidate lies within the dynamic window + if (linear_vel >= dynamic_window.min_linear_vel && + linear_vel <= dynamic_window.max_linear_vel && + angular_vel >= dynamic_window.min_angular_vel && + angular_vel <= dynamic_window.max_angular_vel) + { + // Select the candidate with the largest linear velocity (considering moving direction) + if (linear_vel * sign > best_linear_vel * sign) { + best_linear_vel = linear_vel; + best_angular_vel = angular_vel; + } + } + } + + // If best_linear_vel was updated, it means that a valid intersection exists + if (best_linear_vel != -std::numeric_limits::infinity() * sign) { + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); + } + + // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection, + // select the point within the dynamic window that is closest to the line. + + // Because the dynamic window is a convex region, + // the closest point must be one of its four corners. + const std::array, 4> corners = {{ + {dynamic_window.min_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.min_linear_vel, dynamic_window.max_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.min_angular_vel}, + {dynamic_window.max_linear_vel, dynamic_window.max_angular_vel} + }}; + + // Compute the distance from a point (linear_vel, angular_vel) + // to the line angular_vel = curvature * linear_vel + const double denom = std::sqrt(curvature * curvature + 1.0); + auto compute_dist = [&](const std::array & corner) -> double { + return std::abs(curvature * corner[0] - corner[1]) / denom; + }; + + double closest_dist = std::numeric_limits::infinity(); + best_linear_vel = -std::numeric_limits::infinity() * sign; + best_angular_vel = 0.0; + + for (const auto & corner : corners) { + const double dist = compute_dist(corner); + // Update if this corner is closer to the line, + // or equally close but has a larger linear velocity (considering moving direction) + if (dist < closest_dist || + (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign)) + { + closest_dist = dist; + best_linear_vel = corner[0]; + best_angular_vel = corner[1]; + } + } + + optimal_linear_vel = best_linear_vel; + optimal_angular_vel = best_angular_vel; + + return std::make_tuple(optimal_linear_vel, optimal_angular_vel); +} + +} // namespace dynamic_window_pure_pursuit + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index e6626c2244..962e0101a2 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -31,7 +31,8 @@ namespace nav2_regulated_pure_pursuit_controller struct Parameters { - double desired_linear_vel, base_desired_linear_vel; + double max_linear_vel, base_max_linear_vel, min_linear_vel; + double max_angular_vel, min_angular_vel; double lookahead_dist; double rotate_to_heading_angular_vel; double max_lookahead_dist; @@ -52,7 +53,8 @@ struct Parameters bool use_fixed_curvature_lookahead; double curvature_lookahead_dist; bool use_rotate_to_heading; - double max_angular_accel; + double max_linear_accel, max_angular_accel; + double max_linear_decel, max_angular_decel; bool use_cancel_deceleration; double cancel_deceleration; double rotate_to_heading_min_angle; @@ -62,6 +64,7 @@ struct Parameters bool use_collision_detection; double transform_tolerance; bool stateful; + bool use_dynamic_window; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index fd79208d29..13796357ae 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -32,6 +32,7 @@ #include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" #include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" #include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" +#include "nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -195,6 +196,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool finished_cancelling_ = false; bool is_rotating_to_heading_ = false; bool has_reached_xy_tolerance_ = false; + geometry_msgs::msg::Twist last_command_velocity_; nav2::Publisher::SharedPtr global_path_pub_; nav2::Publisher::SharedPtr carrot_pub_; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 31bc7b7f19..1991a1a7c1 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,8 +36,31 @@ ParameterHandler::ParameterHandler( { plugin_name_ = plugin_name; + // Declare max_linear_vel with backward compatibility + const std::string old_name = plugin_name_ + ".desired_linear_vel"; + const std::string new_name = plugin_name_ + ".max_linear_vel"; + const auto nan_val = std::numeric_limits::quiet_NaN(); declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + node, old_name, rclcpp::ParameterValue(nan_val)); + double old_val = nan_val; + if (node->get_parameter(old_name, old_val) && + !std::isnan(old_val)) + { + RCLCPP_WARN( + logger_, + "Parameter '%s' is deprecated. Use '%s' instead.", + old_name.c_str(), new_name.c_str()); + } + double default_val = std::isnan(old_val) ? 0.5 : old_val; + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(default_val)); + + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(-0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_angular_vel", rclcpp::ParameterValue(-2.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -86,8 +110,14 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( @@ -104,10 +134,15 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(false)); - node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); - params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".max_linear_vel", params_.max_linear_vel); + params_.base_max_linear_vel = params_.max_linear_vel; + node->get_parameter(plugin_name_ + ".min_linear_vel", params_.min_linear_vel); + node->get_parameter(plugin_name_ + ".max_angular_vel", params_.max_angular_vel); + node->get_parameter(plugin_name_ + ".min_angular_vel", params_.min_angular_vel); node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); @@ -162,7 +197,10 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_linear_accel", params_.max_linear_accel); + node->get_parameter(plugin_name_ + ".max_linear_decel", params_.max_linear_decel); node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".max_angular_decel", params_.max_angular_decel); node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration); node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration); node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); @@ -189,6 +227,7 @@ ParameterHandler::ParameterHandler( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); node->get_parameter(plugin_name_ + ".stateful", params_.stateful); + node->get_parameter(plugin_name_ + ".use_dynamic_window", params_.use_dynamic_window); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( @@ -252,9 +291,9 @@ ParameterHandler::updateParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { params_.inflation_cost_scaling_factor = parameter.as_double(); - } else if (param_name == plugin_name_ + ".desired_linear_vel") { - params_.desired_linear_vel = parameter.as_double(); - params_.base_desired_linear_vel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_linear_vel") { + params_.max_linear_vel = parameter.as_double(); + params_.base_max_linear_vel = parameter.as_double(); } else if (param_name == plugin_name_ + ".lookahead_dist") { params_.lookahead_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_lookahead_dist") { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index b348bdf793..809d49dcc6 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2020 Shrijit Singh // Copyright (c) 2020 Samsung Research America +// Copyright (c) 2025 Fumiya Ohnishi // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -231,7 +232,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } - linear_vel = params_->desired_linear_vel; + linear_vel = params_->max_linear_vel; // Make sure we're in compliance with basic constraints // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing @@ -271,7 +272,46 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * regulation_curvature; + if (!params_->use_dynamic_window) { + angular_vel = linear_vel * regulation_curvature; + } else { + // compute optimal path tracking velocity commands + + // considering velocity and acceleration constraints (DWPP) + const double regulated_linear_vel = linear_vel; + + // using last command velocity as a current velocity + const geometry_msgs::msg::Twist current_speed = last_command_velocity_; + + const double & max_linear_vel = params_->max_linear_vel; + const double & min_linear_vel = params_->min_linear_vel; + const double & max_angular_vel = params_->max_angular_vel; + const double & min_angular_vel = params_->min_angular_vel; + + const double & max_linear_accel = params_->max_linear_accel; + const double & max_linear_decel = params_->max_linear_decel; + const double & max_angular_accel = params_->max_angular_accel; + const double & max_angular_decel = params_->max_angular_decel; + + // compute Dynamic Window + dynamic_window_pure_pursuit::DynamicWindowBounds dynamic_window = + dynamic_window_pure_pursuit::computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, + control_duration_); + + // apply regulation to Dynamic Window + dynamic_window_pure_pursuit::applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + + // compute optimal velocity within Dynamic Window + std::tie(linear_vel, angular_vel) = + dynamic_window_pure_pursuit::computeOptimalVelocityWithinDynamicWindow( + dynamic_window, + regulation_curvature, + x_vel_sign); + } } // Collision checking on this velocity heading @@ -292,6 +332,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; + + last_command_velocity_ = cmd_vel.twist; + return cmd_vel; } @@ -388,7 +431,7 @@ void RegulatedPurePursuitController::applyConstraints( params_->approach_velocity_scaling_dist); // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->max_linear_vel); linear_vel = sign * linear_vel; } @@ -406,14 +449,14 @@ void RegulatedPurePursuitController::setSpeedLimit( if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value - params_->desired_linear_vel = params_->base_desired_linear_vel; + params_->max_linear_vel = params_->base_max_linear_vel; } else { if (percentage) { // Speed limit is expressed in % from maximum speed of robot - params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0; + params_->max_linear_vel = params_->base_max_linear_vel * speed_limit / 100.0; } else { // Speed limit is expressed in absolute value - params_->desired_linear_vel = speed_limit; + params_->max_linear_vel = speed_limit; } } } diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index dc146c36cc..3f8f5ed6f2 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ # tests for regulated PP nav2_add_gtest(test_regulated_pp test_regulated_pp.cpp + test_dwpp_functions.cpp path_utils/path_utils.cpp ) target_link_libraries(test_regulated_pp diff --git a/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp new file mode 100644 index 0000000000..ebca6673fd --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_dwpp_functions.cpp @@ -0,0 +1,280 @@ +// Copyright (c) 2025 Fumiya Ohnishi +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp" +#include "gtest/gtest.h" + +using namespace nav2_regulated_pure_pursuit_controller::dynamic_window_pure_pursuit; // NOLINT + + +TEST(DynamicWindowPurePursuitTest, computeDynamicWindow) +{ + double max_linear_vel = 0.5; + double min_linear_vel = -0.5; + double max_angular_vel = 1.0; + double min_angular_vel = -1.0; + double max_linear_accel = 0.5; + double max_linear_decel = 1.0; + double max_angular_accel = 1.0; + double max_angular_decel = 2.0; + double control_duration = 1.0 / 20.0; + + DynamicWindowBounds dynamic_window; + geometry_msgs::msg::Twist current_speed = geometry_msgs::msg::Twist(); + + // case1: current linear velocity is positive, angular velocity is positive, + // clip by max linear vel + current_speed.linear.x = 0.5; current_speed.angular.z = 0.2; + + dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window.max_linear_vel, 0.5); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.45); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window.min_angular_vel, 0.10); + + // case2: current linear velocity is positive, angular velocity is negative + current_speed.linear.x = 0.3; current_speed.angular.z = -0.2; + + dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window.max_linear_vel, 0.325); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.25); + EXPECT_EQ(dynamic_window.max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.25); + + // case3: current linear velocity is zero, angular velocity is zero + current_speed.linear.x = 0.0; current_speed.angular.z = -0.0; + + dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window.max_linear_vel, 0.025); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.025); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.05); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.05); + + // case4: current linear velocity is negative, angular velocity is positive + current_speed.linear.x = -0.3; current_speed.angular.z = 0.2; + + dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window.max_linear_vel, -0.25); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.325); + EXPECT_EQ(dynamic_window.max_angular_vel, 0.25); + EXPECT_EQ(dynamic_window.min_angular_vel, 0.10); + + // case5: current linear velocity is negative, angular velocity is negative, + // clipped by min lenear vel + current_speed.linear.x = -0.5; current_speed.angular.z = -0.2; + + dynamic_window = computeDynamicWindow( + current_speed, max_linear_vel, min_linear_vel, + max_angular_vel, min_angular_vel, + max_linear_accel, max_linear_decel, + max_angular_accel, max_angular_decel, control_duration); + + EXPECT_EQ(dynamic_window.max_linear_vel, -0.45); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.5); + EXPECT_EQ(dynamic_window.max_angular_vel, -0.1); + EXPECT_EQ(dynamic_window.min_angular_vel, -0.25); +} + +TEST(DynamicWindowPurePursuitTest, applyRegulationToDynamicWindow) +{ + double regulated_linear_vel; + DynamicWindowBounds dynamic_window; + + regulated_linear_vel = 0.3; + + dynamic_window.max_linear_vel = 0.5; + dynamic_window.min_linear_vel = 0.4; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.4); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.4); + + dynamic_window.max_linear_vel = 0.4; + dynamic_window.min_linear_vel = 0.2; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.3); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.2); + + dynamic_window.max_linear_vel = 0.2; + dynamic_window.min_linear_vel = 0.1; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.1); + + dynamic_window.max_linear_vel = 0.1; + dynamic_window.min_linear_vel = -0.2; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.0); + + dynamic_window.max_linear_vel = -0.1; + dynamic_window.min_linear_vel = -0.3; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.1); + + regulated_linear_vel = -0.3; + + dynamic_window.max_linear_vel = 0.3; + dynamic_window.min_linear_vel = 0.1; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.1); + EXPECT_EQ(dynamic_window.min_linear_vel, 0.1); + + dynamic_window.max_linear_vel = 0.1; + dynamic_window.min_linear_vel = -0.2; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, 0.0); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.2); + + dynamic_window.max_linear_vel = -0.2; + dynamic_window.min_linear_vel = -0.3; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.3); + + dynamic_window.max_linear_vel = -0.2; + dynamic_window.min_linear_vel = -0.4; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.2); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.3); + + dynamic_window.max_linear_vel = -0.4; + dynamic_window.min_linear_vel = -0.5; + applyRegulationToDynamicWindow( + regulated_linear_vel, + dynamic_window); + EXPECT_EQ(dynamic_window.max_linear_vel, -0.4); + EXPECT_EQ(dynamic_window.min_linear_vel, -0.4); +} + +TEST(DynamicWindowPurePursuitTest, computeOptimalVelocityWithinDynamicWindow) +{ + DynamicWindowBounds dynamic_window; + double optimal_linear_vel; + double optimal_angular_vel; + + double sign = 1.0; + + double curvature = 0.0; + + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.1, dynamic_window.min_angular_vel = -0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.0); + + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + + dynamic_window.max_linear_vel = 0.2, dynamic_window.min_linear_vel = 0.0; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.2); + EXPECT_EQ(optimal_angular_vel, 0.2); + + dynamic_window.max_linear_vel = 0.4, dynamic_window.min_linear_vel = 0.3; + dynamic_window.max_angular_vel = 0.2, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); + + sign = -1.0; + + curvature = 0.0; + + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.1, dynamic_window.min_angular_vel = -0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.0); + + dynamic_window.max_linear_vel = 0.3, dynamic_window.min_linear_vel = 0.1; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + curvature = 1.0; + + dynamic_window.max_linear_vel = 0.2, dynamic_window.min_linear_vel = 0.0; + dynamic_window.max_angular_vel = 0.3, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.1); + EXPECT_EQ(optimal_angular_vel, 0.1); + + dynamic_window.max_linear_vel = 0.4, dynamic_window.min_linear_vel = 0.3; + dynamic_window.max_angular_vel = 0.2, dynamic_window.min_angular_vel = 0.1; + std::tie(optimal_linear_vel, + optimal_angular_vel) = computeOptimalVelocityWithinDynamicWindow( + dynamic_window, curvature, sign); + EXPECT_EQ(optimal_linear_vel, 0.3); + EXPECT_EQ(optimal_angular_vel, 0.2); +} diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 6e1ad1eec7..ec343f9b3c 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -35,7 +35,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return params_->desired_linear_vel;} + double getSpeed() {return params_->max_linear_vel;} std::unique_ptr createCarrotMsgWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -439,7 +439,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("test.desired_linear_vel", 1.0), + {rclcpp::Parameter("test.max_linear_vel", 1.0), rclcpp::Parameter("test.lookahead_dist", 7.0), rclcpp::Parameter("test.max_lookahead_dist", 7.0), rclcpp::Parameter("test.min_lookahead_dist", 6.0), @@ -468,7 +468,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) node->get_node_base_interface(), results); - EXPECT_EQ(node->get_parameter("test.desired_linear_vel").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.max_linear_vel").as_double(), 1.0); EXPECT_EQ(node->get_parameter("test.lookahead_dist").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.max_lookahead_dist").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.min_lookahead_dist").as_double(), 6.0);