Nav2 Regulated Pure Pursuit set with Ackermann model skips driving maneuvers given by path planner

[andreacelani]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04.5
• ROS2 Version:
  • Humble
• Version or commit hash:
  • 1.1.16
• DDS implementation:
  • rmw_fastrtps_cpp

Issue Description:

Nav2 Regulated Pure Pursuit set with Ackermann model skips driving maneuvers given by path planner. It jumps to the farthest point in the lookahaed radius skipping fundamental maneuvers to follow global path .

Steps to reproduce issue

1- Configure an Ackermann steering vehicle with the Nav2 stack, kinematically feasible planner, like Smac Planner, and RPP controller. In my case the robot model is an Ackermann based robot of a real agricultural tractor. Wheel base around 2 meters, minimum turning radius 4.2 meters, track 1.6 meters rear and 1.4 meters front.

2- Plan a path without re-planning (one single plan).

3- Observe the vehicle's behavior while executing the planned path, particularly during the reverse segments.

Here are the parameters of controller.
Note: these settings turn on the use_regulated_linear_velocity_scaling and the min_lookahead_dist has been set to very low value. The aim is to show that even if the controller could search with very limited horizon it will skip large portion of planned path.

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.01
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.01
    progress_checker_plugins: ["progress_checker"]  # progress_checker_plugin: "progress_checker" For Humble and older
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.1
      yaw_goal_tolerance: 0.05
      stateful: True
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.25
      lookahead_dist: 5.0
      min_lookahead_dist: 0.1
      max_lookahead_dist: 5.0
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.8
      transform_tolerance: 0.5
      use_velocity_scaled_lookahead_dist: true
      min_approach_linear_velocity: 0.1
      approach_velocity_scaling_dist: 0.6
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 2.0
      use_regulated_linear_velocity_scaling: true
      use_fixed_curvature_lookahead: false
      curvature_lookahead_dist: 1.5
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 5.0
      regulated_linear_scaling_min_speed: 0.15
      use_rotate_to_heading: false
      allow_reversing: true
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 3.2
      max_robot_pose_search_dist: 10.0

And parameters of planner

  ros__parameters:
    expected_planner_frequency: 2.0
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      downsample_costmap: False                     # whether or not to downsample the map
      downsampling_factor: 1                        # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      tolerance: 0.25                               # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      allow_unknown: True                           # allow traveling in unknown space
      max_iterations: 1000000                       # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000              # Maximum number of iterations after within tolerances to continue to try to find exact solution
      max_planning_time: 5.0                        # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP"        # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72                   # Number of angle bins for search
      analytic_expansion_ratio: 3.5                 # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 16.0            # If the length is too far, reject this expansion. This prevents shortcutting of search with its penalty functions far out from the goal itself (e.g. so we don’t reverse half-way across open maps or cut through high cost zones). This should never be smaller than 4-5x the minimum turning radius being used, or planning times will begin to spike.
      analytic_expansion_max_cost: 200.0            # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: False   #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      minimum_turning_radius: 4.7                   # minimum turning radius in m of path / vehicle
      reverse_penalty: 1.1                          # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                           # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.2                     # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 8.0                             # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.015                  # Heuristic penalty to apply to SE2 node penalty. Causes Hybrid-A* to prefer later maneuvers before earlier ones along the path. Saves search time since earlier (shorter) branches are not expanded until it is necessary. Must be >= 0.0 and <= 1.0. Must be 0.0 to be fully admissible.
      lookup_table_size: 20.0                       # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: False               # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      debug_visualizations: False                   # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      use_quadratic_cost_penalty: False
      downsample_obstacle_heuristic: True
      allow_primitive_interpolation: False
      smooth_path: False                             # If True, does a simple and quick smoothing post-processing to the path
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: True
        refinement_num: 2

Expected behavior

The controller should follow more strictly every single segments (points) of planned path in forward direction and reversed direction.

Actual behavior

the purple point is the lookahead target.
The simulation is pretty large and the realtime ratio is 15%. This explains the somewhat chomping progress of the video

The robot start going backward, then try to follow strictly the planned forward path but at given point it skips to the longest segment.
The final trajectory is far from what has been planned by Smac planner and it hits the obstacles (actually it stops since use_collision_detection is set to True)

test2.webm

Additional information

It looks like the lookahead algorithm push the controller to choose a shortcut over the complete planned path. It ignores the chronological order of planned points. The pruning to the closest point could be the source of the unexpected behavior.

I tested also MPPI and it follows the driving maneuvers "more strictly". By the way I prefer RPP since MPPI is too complex to tune. At this time I didn't find a set of parameters that reduce the noisy steering behavior that MPPI has. Don't get me wrong the MPPI is far more complex and I suppose far better than RPP but I cannot make it to work efficenty.

[SteveMacenski]

I tested also MPPI and it follows the driving maneuvers "more strictly".

This is a really helpful comment.

Can you look at the path pruning behavior between them and identify the difference? I assume there's some cusp detection in MPPI that isn't in RPP (yet) and it may be just as easy as porting it over to RPP as well. MPPI's path handler was originally based on RPP's, so any changes should be pretty straight forward to migrate over and test!

Highly relevant files:

• https://github.com/ros-navigation/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp
• https://github.com/ros-navigation/navigation2/blob/main/nav2_mppi_controller/src/path_handler.cpp

[andreacelani]

@SteveMacenski you got the point (of course I would say). I'm checking it but in the meantime I found this

while MPPI has path_handler.cpp in humble branch.

Does it is supposed to be like that?

[SteveMacenski]

It looks like it is in the controller still in Humble (https://github.com/ros-navigation/navigation2/blob/humble/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp) - so I'd recommend doing it on main (since we only merge PRs on main so that they are in all new distributions and backported whereever possible)

You could, in concept, work on humble, but then we'd need to forward port it to main before it could be merged. Given main looks most like RPP, it seems most straight forward to me (but either works!)

[andreacelani]

To me looks like the RPP clamps the furthest point of local plan with this

navigation2/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp

Lines 67 to 69 in e52ea5d

	auto closest_pose_upper_bound =
	nav2_util::geometry_utils::first_after_integrated_distance(
	global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist);

while MPPI use basically the same approach but make use of global_plan_up_to_inversion_ instead of global_plan_

navigation2/nav2_mppi_controller/src/path_handler.cpp

Lines 57 to 60 in e52ea5d

	auto closest_pose_upper_bound =
	nav2_util::geometry_utils::first_after_integrated_distance(
	global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
	max_robot_pose_search_dist_);

with this:

navigation2/nav2_mppi_controller/src/path_handler.cpp

Line 134 in e52ea5d

inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);

and parameters should be added to RPP as well as we find in MPPI:

navigation2/nav2_mppi_controller/src/path_handler.cpp

Lines 40 to 44 in e52ea5d

	getParam(enforce_path_inversion_, "enforce_path_inversion", false);
	if (enforce_path_inversion_) {
	getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
	getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
	inversion_locale_ = 0u;

[SteveMacenski]

Seems reasonable! Can you test that out and submit a PR if it resolves your issue? :- )

[andreacelani]

yes, let me try.

[andreacelani]

@SteveMacenski in regulated_pure_pursuit_controller.cpp I found this:

navigation2/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Lines 494 to 536 in e52ea5d

	double RegulatedPurePursuitController::findVelocitySignChange(
	const nav_msgs::msg::Path & transformed_plan)
	{
	// Iterating through the transformed global path to determine the position of the cusp
	for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) {
	// We have two vectors for the dot product OA and AB. Determining the vectors.
	double oa_x = transformed_plan.poses[pose_id].pose.position.x -
	transformed_plan.poses[pose_id - 1].pose.position.x;
	double oa_y = transformed_plan.poses[pose_id].pose.position.y -
	transformed_plan.poses[pose_id - 1].pose.position.y;
	double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
	transformed_plan.poses[pose_id].pose.position.x;
	double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
	transformed_plan.poses[pose_id].pose.position.y;
	/* Checking for the existance of cusp, in the path, using the dot product
	and determine it's distance from the robot. If there is no cusp in the path,
	then just determine the distance to the goal location. */
	const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y);
	if (dot_prod < 0.0) {
	// returning the distance if there is a cusp
	// The transformed path is in the robots frame, so robot is at the origin
	return hypot(
	transformed_plan.poses[pose_id].pose.position.x,
	transformed_plan.poses[pose_id].pose.position.y);
	}
	if (
	(hypot(oa_x, oa_y) == 0.0 &&
	transformed_plan.poses[pose_id - 1].pose.orientation !=
	transformed_plan.poses[pose_id].pose.orientation)
	||
	(hypot(ab_x, ab_y) == 0.0 &&
	transformed_plan.poses[pose_id].pose.orientation !=
	transformed_plan.poses[pose_id + 1].pose.orientation))
	{
	// returning the distance since the points overlap
	// but are not simply duplicate points (e.g. in place rotation)
	return hypot(
	transformed_plan.poses[pose_id].pose.position.x,
	transformed_plan.poses[pose_id].pose.position.y);
	}
	}

it seems the cusp detection in RPP is handled by controller.cpp and not by path_handler.cpp. I will work on this rather than path_handler.cpp

[andreacelani]

Seems reasonable! Can you test that out and submit a PR if it resolves your issue? :- )

ok, made it. I attached a short video in PR to show how things works.
The issue was mainly related to how the starting point of pruned path was processed. Originally it's processed on the complete path, so if you have a path that intersecting itself or several driving maneuvers close each other and noise in odometry the RPP suddenly skips to next segments and usually to the last segment nullifying the kinematically feasibility effort made by the planner.
Any comments or thoughts are welcome.