Testing Regulated Pure Pursuit controller - allow reversing feature

[Naraharirahul]

Hello, based on the #2551 I have been trying to test the allow-reversing feature on the turtlebot3 waffle environment (on Galactic Sync 3 debian), but I have not been able to do so. I have used the following controller parameters which is almost similar to the one here

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "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.25
      yaw_goal_tolerance: 0.25
      stateful: True
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.05
      max_linear_accel: 2.5
      max_linear_decel: 2.5
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.8
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      use_approach_linear_velocity_scaling: true
      max_allowed_time_to_collision: 1.0
      use_regulated_linear_velocity_scaling: true
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: false
      allow_reversing: true
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 0.5

I get the following error whenever I give the path behind the robot without changing the heading direction (This step should use allow-reversing feature)

The dimensions of the costmap is too small to successfully check for collisions as far ahead as requested. Proceed at your own risk, slow the robot, or increase your costmap size.
I have reduced the linear velocity as well as angular velocity of the robot and also increased the costmap size to [width : 7 height ; 7] which made the above error go, but I get another error

RegulatedPurePursuitController detected collision ahead!

I would like to know the environment in which you have tested and any changes you would recommend to my controller parameters to make the allow reversing work for the regulated pure pursuit controller.

[SteveMacenski]

https://github.com/ros-planning/navigation2/blob/1.0.6/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Galactic sync 3 does not contain reversing.

[Naraharirahul]

Sorry I meant the galactic branch. It does have a reversing part.

[SteveMacenski]

Reversing uses detected cusps of direction changes to indicate directionality change, like that in which is provided by the smac Hybrid-A* planner, such as shown below

[Naraharirahul]

Thanks for response. I understand that, I need to get the cusp closer to the end goal in order to not change the direction of the robot. Could you help me understand what parameter in controller / planner or both needs to be tuned to control the cusp.