RPP cusp not detected for in-place rotation (for reversing)

[automech-rb]

Bug report

Required Info:

• Operating System:
  • Docker 20.04 Ubuntu
• ROS2 Version:
  • rolling
• Version or commit hash:
  • latest 104bdc5
• DDS implementation:
  • rmw_cyclonedds_cpp

Steps to reproduce issue

Use SMAC-lattice planner with in-place rotation on
and set use_rotate_to_heading: false and allow_reversing: true in RPP

  FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.8
      lookahead_dist: 0.4
      min_lookahead_dist: 0.4
      max_lookahead_dist: 0.8
      lookahead_time: 1.0
      rotate_to_heading_angular_vel: 0.5
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 0.8
      max_allowed_time_to_collision_up_to_carrot: 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.2
      use_rotate_to_heading: false
      allow_reversing: true
      rotate_to_heading_min_angle: 0.785 #45degree
      max_angular_accel: 0.5
      max_robot_pose_search_dist: 10.0
      use_interpolation: false

The above figure shows a cusp with in-place rotation.

Expected behavior

The lookup should first be till the cusp and after robot arrives at cusp it should lookup to next point.

Actual behavior

Cusp is not detected and robot lookup directly crosses the cusp even when robot has not arrived to the cusp pose.

Additional information

This is due to cusp being detected by negative dot product of consecutive pose on this line. As the in-place rotation have multiple consecutive pose at same location, the dot product is zero and cusp is not detected.

Maybe instead of dot product < 0, changing it to <= 0 may help. But it will bring along additional bugs such as

• When path starts with in-plane rotation: this will lead to lookup stuck at start (tested)
• false cusp detection in below case (confirmed SMAC-lattice produce such paths sometimes).
  
  Sorry for my bad drawing skills!

[automech-rb]

Maybe its better to just skip the in-place rotation poses with same (x,y) [while keeping the first and the last of those consecutive pose] in the cusp detection loop.

Best option would be to support RotateToGoalHeading & RotateToPath along with allow_reversing for differential-robot at same time.

Any thoughts?

[SteveMacenski]

I think you're spot on with the difference between rotate_to_heading and rotate_along_path. The thought process here is that we wanted to make it optional if the robot was able to meaningfully rotate to heading of a path (for the case of a diff drive robot with a holonomic planner asking it to move on the left side rather than starting from the current heading). While for other cases, the paths (like from Smac) are feasible and will specify changes in direction and we don't want the robot to rotate towards that heading in place. Also, some drive trains like Ackermann cannot rotate in place at all.

rotate_along_path has a similar meaning, some can do it but others cannot, so it makes sense to have this also be an option.

I think it could even fit in really well into the existing structure to add in that parameter + a new shouldRotateAlongPath method inserted into the main logic around ~L337 (where rotate to goal / path is located). Here, we can check if rotate_along_path_ is true & if there is an in-place rotation called out in the path due to something like the State Lattice planner. For example, checking if multiple pose are in the same spot with different angles, rotate towards the last one. You could even use the same rotateToHeading method as we have for the other path/goal rotation methods, it seems like a very natural fit.

There would probably need to be a slight change in the cusp calculation to also return for in place rotations, but that should be easy to check for as you point out with <= 0 -- or have it handled within shouldRotateAlongPath and cusp detection doesn't need to change. For the "false cusp" diagram above, I'd argue that you probably should still detect that, and instead of changing directions, it should stop and rotate there like it requests. Or ignore it, but then its pretty hard to tell the differences between in-place rotations it should respect (like the first image) and others it should ignore (like the hand-drawn diagram). I don't think we can have it both ways if rotation_along_path is set to true.

Would this be something you'd be open to contributing?

[automech-rb]

Hello @SteveMacenski. Sorry for the late reply.
Now thinking back, maybe using <= 0 is better idea to correctly follow the path (even the false cusp in my hand drawn picture).
I also agree with shouldRotateAlongPath method for allow_reversing parameter. To add, the sign calculation method needs to be updated.
The problem with current sign calculation is that it assumes there are no in-place rotation as shown below:
https://github.com/ros-planning/navigation2/blob/104bdc51fb72e05af4ccb4fb96a2fd421239f016/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L325
If robot is not in correct orientation (due to in-place rotation) when calculating carrot pose, it leads to opposite sign value.

I actually did some testing few weeks ago with the above modifications, but now I am not actively working with this planner. If my boss let me work on this in future, I will submit a pr.
If you like, I can close this issue for now.

[SteveMacenski]

This is still a valid issue, so I want to keep this open!

[SteveMacenski]

#3934 to resolve