RPP planner strange behaviour in reverse

[jainRAV]

Bug report

Required Info:

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

Steps to reproduce issue

Setting use_rotate_to_heading: false and allow_reversing: true

  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

Expected behavior

Robot to follow path in reverse (backward motion)

Actual behavior

Robot rotate completely and follow the path in strange fashion (as shown in gif above)

Additional information

In gif, the face with additional frame at front is front.

[automech-rb]

After removing sign * from this line, it works perfectly.

Looks like this bug was introduced by this PR 2979. The above PR seems to be testing for ackermann while the above reported bug is on differential robot leading to strange behavior!

[SteveMacenski]

@afonsofonbraga can you respond to this from https://github.com/ros-planning/navigation2/pull/2979?

I agree, that the sign is already being applied in the applyConstraints as an input, so if we apply it again on angular_vel it would be canceling it out. That wouldn't matter if it were positive because +1 * +1 = +1 but would if negative since -1 *-1 = +1.

On closer inspection, I don't see how #2979 makes sense and I would think reverting it would be reasonable. @automech-rb can you submit a PR? I'd like @afonsofonbraga on the topic so we can find a solution to both at the same time, I see no reason why this should vary for diff drive / ackermann robots. Maybe @afonsofonbraga had something misconfigured like you show above with reversing enabled / rotate in place disabled? Their initial gif looks like the robot is trying to (poorly) rotate in place, so it could be that they did not set use_rotate_to_heading: false and allow_reversing: true. Though publishing the trajectory arc + lookahead point in rviz would help clear that up a bit.

[automech-rb]

Yeah. Probably the reason is some misconfigured setup. Okay 👍 I will submit the PR.

[afonsofonbraga]

Hi, @SteveMacenski and @automech-rb I'll test this at the end of the week and check the params I used at the time. From what I remember the configs were also use_rotate_to_heading: false and allow_reversing: true

[afonsofonbraga]

Hi guys, I reversed the commit and used the same parameters above. With https://github.com/ros-planning/navigation2/pull/2979 the Ackermann works fine and without it not. Here is the gif without the commit:

One thing I realized here was that the gazebo tricycle model works with angular commands and not angular speed, that's why it was reversed and I didn't realize it. On TEB there is an option to convert the angular speed to angle for gazebo. Do you guys think it is useful to add it to the pure pursuit to avoid this behavior? This next git shows that the steering angle remains the same when reversing the linear speed. I hope I was able to answer and clarify the mistake I did. I'll switch to the angular speed control on gazebo and check how the system will behave.

[SteveMacenski]

@afonsofonbraga thanks for the testing / context! So to be clear, the issue is with the gazebo tricycle model + RPP (or I suppose any controller) in this context because the twist angle command means different things? Thus, I think this should be reverted for everyone else and have another ticket / issue around the tricycle model support.

I'll switch to the angular speed control on gazebo and check how the system will behave.

How did your testing go?

[SteveMacenski]

Closing issue from fix thanks @automech-rb @jainRAV @afonsofonbraga for your time and effort on debugging this! I backported the changes to Galactic and Humble, I'll plan to run a sync either tomorrow or next week, I want to get through the bulk of my notifications from PTO before doing it to include as much relevant stuff as possible.

@afonsofonbraga can you file a new ticket around the tricycle model? Or do you think that changing gazebo will work for you?

[afonsofonbraga]

Hi @SteveMacenski, my solution was to create a node that converts the RPP angular speed output into steering angle and feeds it to the gazebo tricycle model. It's working fine now. In May, I submitted PR on gazebo_ros_pkgs fixing bugs on the gazebo plugin but didn't get any answers. It is working for me now but if you think this is a great feature to be added into RPP: the possibility of sending also steering angles I can create a ticket and implement it to nav2. 😊