search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.61k stars 1.3k forks source link

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

Closed automech-rb closed 1 year ago

automech-rb commented 2 years ago

Bug report

Required Info:

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

image 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

automech-rb commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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

SteveMacenski commented 1 year ago

https://github.com/ros-planning/navigation2/pull/3934 to resolve