RPP produces discontinuous angular velocities when the linear one is limited #2621
Description
Bug report
Required Info:
- Operating System:
- Kubuntu 20.04
- ROS2 Version:
- Galactic binaries
- Version or commit hash:
- DDS implementation:
- default
Steps to reproduce issue
Controller server parameters:
`controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 30.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.15
stateful: True
# DWB parameters
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.25
max_linear_accel: 0.3
max_linear_decel: 0.3
lookahead_dist: 0.9
min_lookahead_dist: 0.3
max_lookahead_dist: 1.5
lookahead_time: 1.5
rotate_to_heading_angular_vel: 0.175
transform_tolerance: 0.1
use_velocity_scaled_lookahead_dist: true
min_approach_linear_velocity: 0.05
use_approach_linear_velocity_scaling: true
max_allowed_time_to_collision: 5.0
use_regulated_linear_velocity_scaling: true
regulated_linear_scaling_min_radius: 0.7
regulated_linear_scaling_min_speed: 0.05
allow_reversing: true
use_rotate_to_heading: false
rotate_to_heading_min_angle: 0.35
max_angular_accel: 0.05
use_cost_regulated_linear_velocity_scaling: false`
Expected behavior
max_linear_accel and max_linear_decel (maybe max_angular_accel?) limit accelerations so that the twist control commands have a smooth shape and continuous shape.
Actual behavior
When setting max_linear_accel (and decel) parameters to relatively small values (0.3 concretely for the attached plot) so that the velocity profile has a shape ramp, the controller may send discontinuous angular velocity commands, particularly in the vicinity of a cusp in the path.
Additional information
Using Regulated Pure Pursuit controller on an omnidirectional robot in gazebo simulation. Such discontinuous commands usually causes the robot to get lost and thus not being able to continue navigating.