RPP controller feature: having allow_reversing and use_rotate_to_heading settable to true

[Axel927]

Feature request

Context

Hello, I am using the RPP controller (version Humble) for a differential robot that can rotate in place. The path planner used is NavFn. In my configuration, I use use_rotate_to_heading so the robot can rotate to the expected heading when close to the goal position. I have some use cases where I need to go backward, but as I use use_rotate_to_heading, I can't. I also need to use use_rotate_to_heading because my robot have to go in narrow spaces so it can't do large movements to get the expected rotation.

Feature description

Being able to have use_rotate_to_heading and allow_reversing set to true.

Implementation considerations

The documentation says that this would result to ambiguous situations, but I actually don't see any.

I implemented the possibility to use use_rotate_to_heading and allow_reversing to see if I could see an ambiguous situation while in use and the answer is no.

Modifications applied for testing

• I removed the checks to avoid having both use_rotate_to_heading and allow_reversing set to true in the methods configure and dynamicParametersCallback
• I declared a new parameter named max_rotation_before_reverse
• I modified the method shouldRotateToPath

  bool RegulatedPurePursuitController::shouldRotateToPath(
  const geometry_msgs::msg::PoseStamped & carrot_pose, 
  double & angle_to_path
  )
  {
  // Whether we should rotate robot to rough path heading
  angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
  return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_ 
      && (!allow_reversing_ || fabs(angle_to_path) < max_rotation_before_reverse_); // DIFF
  }

Test results

max_rotation_before_reverse_ value: 2.2

Image of the start position:

Behavior: The robot rotates before following the path. If max_rotation_before_reverse_ value is below 2.2, the robot goes backward instead of rotating.

Easier start position: When the path is behind the robot, it goes backward as expected.

Conclusion: I the tested cases I have no issue having use_rotate_to_heading and allow_reversing set to true.

[SteveMacenski]

Hi thanks for the detailed information and testing, that makes this discussion much easier.

I have some use cases where I need to go backward, but as I use use_rotate_to_heading, I can't. I also need to use use_rotate_to_heading because my robot have to go in narrow spaces so it can't do large movements to get the expected rotation.

How would you ideally want to make the decision about when to rotate to the path's heading vs reversing? I suppose that's the crux of why I thought of these as creating ambiguous situations.

I imagine 3 situations:

(1) You have a robot with no sensor coverage backward --> In this case, you'd never want to go backward, so you wouldn't need to have both true. You'd want to rotate.

(2) You have a robot that is fully bidirectional --> In this case, you'd never want to rotate to heading, probably. So no need to have both set, you'd just want to reverse.

(3) You have a robot with main sensor coverage forward but some limited in reverse --> In this case, you probably want to move forward assuming forward is "reasonable". But the rules about whether you go forward or reverse seems context dependent and not just about which is angularly closer (i.e. if path is very long, you wouldn't want to reverse for 100m; if in very congested area also wouldn't want to reverse; but for short distances, this would be good). In that situation, its the planner (I conjecture) that would make some decision about when to reverse or go forward, which the findVelocitySignChange work for cusp detection on feasible planning would come into play & then we set the reverse behavior to true so that we can respect the path as given.

I'm not sure I see it as easy as just what is closer from within a threshold. But I would love to hear about your situation where you want to have a threshold-based decision about rotating or reversing to know if there's a 4th situation I'm not considering and why using global planning to decide forward/reverse is inappropriate.

---

Also, what about the following which would apply if both are set. We would check if we are being asked to reverse & set the direction of travel appropriately. Then, we'd be using shouldRotateToPath not based off of the original direction, but the updated one. So the threshold you specify would need to consider that.

  if (params_->allow_reversing) {
    x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
  }

In the main branch, that function looks different that we'd need to contend with.

[Axel927]

In my situation, the robot can equally move forward and backward with a LiDAR at the top with a 360° visibility. It also can rotate perfectly centered. I only have short distances to move (less than 4 meters).

In most displacement cases, having the robot to rotate in place and then follow the path forward works fine. I have goal position where the robot have to move into a narrow corridor (barely larger than the robot) and once the position is reached, it rotates to have to goal heading. As there is not much room for the robot, using use_rotate_to_heading is a must have. Sometimes, the next goal position is 0.5m behind with the same heading, so the best move is to move backward as it doesn't have to rotate twice of 180°. This is a case where being able to also reverse would be good, and probably the only one.

In my case, robot being symmetric in capability of movement. A good trigger to choose between backward and forward would be the absolute angle of the path (> 90° -> backward and <= 90° -> forward) and a second trigger to rotate or not would be rotate_to_heading_min_angle applied for both a forward and a backward direction (applied around PI rad instead of 0 for backward). This is actually not what I tested, but I believe it would be better than what I firstly did.

Examples: Robot heading = 0 rad rotate_to_heading_min_angle = 0.785 rad (note: angle_to_path refer to the variable in the method shouldRotateToPath)

1) abs angle_to_path = 0.6 rad Result: robot goes forward

2) abs angle_to_path = 1.2 rad Result: robot rotates until reaching an angle of 0.415 rad and then start moving forward

3) abs angle_to_path = 2.2 rad Result: robot rotates until reaching an angle of -1.415 rad and then start moving backward

4) abs angle_to_path = 3.0 rad Result: robot goes backward

Like this, I believe it should work, except if I am missing something. Maybe I am in a too much edge case and I am the only one needing this, so it would not be interesting to make it available. Anyway, I will try to do something like that for my project.

[SteveMacenski]

Sometimes, the next goal position is 0.5m behind with the same heading, so the best move is to move backward as it doesn't have to rotate twice of 180°. This is a case where being able to also reverse would be good, and probably the only one.

So reverse :+1: I get that with your corridor setup.

In most displacement cases, having the robot to rotate in place and then follow the path forward works fine.

This is where you lose me, just because it works fine doesn't mean you couldn't just have it reverse, given the symmetric nature of the robot! Arguably, that would be more optimal because it would be faster and therefore the robot can complete more tasks. I think you may be making this harder on yourself by calling this behavior 'good enough' when in fact you can get a better behavior which removes the issue that 'good enough' creates :-)

Maybe I am in a too much edge case and I am the only one needing this, so it would not be interesting to make it available.

I don't say this isn't a good contribution, but I think perhaps that you should re-evaluate the 'works fine' statement as possibly actually making your system more complicated, thinking its simpler. If after reflection this behavior is truly needed and isn't just because its 'good enough' and is actually the requirements-driven best optimal behavior you want, then we can talk about how to get that contribution into the stack. We should be able to accommodate it.

[Axel927]

This is where you lose me, just because it works fine doesn't mean you couldn't just have it reverse, given the symmetric nature of the robot! Arguably, that would be more optimal because it would be faster and therefore the robot can complete more tasks. I think you may be making this harder on yourself by calling this behavior 'good enough' when in fact you can get a better behavior which removes the issue that 'good enough' creates :-)

Sorry, it seems my formulation led to a misunderstanding. What I meant was that just rotate to heading was good enough for most situations I encounter. However I have some cases in which rotating wastes a lot of time and moving backward would be more optimal. For an example, I have a gripper in the front which pick up cans and have to let them down 0.5 m behind.  Let's suppose I have use_rotate_to_heading set to false for the above example. However sometimes, the robot has to rotate 90° in narrow spaces so it has to rotate in place to not hurt an obstacle. However, if use_rotate_to_heading is false, the robot went backward and forward to rotate like an Ackermann robot. But maybe I misconfigured?

[SteveMacenski]

Might I recommend using a feasible planner like the Smac Planner? This will plan the direction changes in order to make sure you don't collide while in confined spaces by taking into account the full robot footprint (rather than a circular bounding footprint assumption). That is nice with non-circular robots, especially ones that need to back up so that the path inversions and so forth are explicitly planned to get where you need to be. You could use Hybrid-A* or State Lattice if you wanted to still plan with rotate in places.

That would resolve the 90 degree cases, since the feasible planners consider the robot's starting orientation in planning, so there's never a situation where the plan starts off at some angle offset wrt the robot's orientation. Its a pretty common pairing for RPP :-) Using that planner, then you wouldn't ever require rotating to heading by definition of the feasible planner's output. The planner itself would contain that action.

[Axel927]

I can't use the Smac Planner due to an error about bad allocation at launch (see below). I have this error for the hybrid and the lattice. The 2D has no error. I particularly used the example configuration for humble.

[component_container_isolated-1] [INFO] [1731403201.210448595] [planner_server]: Created global planner plugin GridBased of type nav2_smac_planner/SmacPlannerHybrid
[component_container_isolated-1] [INFO] [1731403201.210525182] [planner_server]: Configuring GridBased of type SmacPlannerHybrid
[component_container_isolated-1] [INFO] [1731403201.214850731] [planner_server]: Even sized heuristic lookup table size set 20000.000000, increasing size by 1 to make odd
[component_container_isolated-1] [INFO] [1731403201.215151427] [planner_server]: Destroying plugin GridBased of type SmacPlannerHybrid
[component_container_isolated-1] [ERROR] [1731403201.215265826] [planner_server]: Caught exception in callback for transition 10
[component_container_isolated-1] [ERROR] [1731403201.215277860] [planner_server]: Original error: std::bad_alloc
[component_container_isolated-1] [WARN] [1731403201.215295628] [planner_server]: Error occurred while doing error handling.
[component_container_isolated-1] [FATAL] [1731403201.215311019] [planner_server]: Lifecycle node planner_server does not have error state implemented
[component_container_isolated-1] [ERROR] [1731403201.215689020] [lifecycle_manager_navigation]: Failed to change state for node: planner_server
[component_container_isolated-1] [ERROR] [1731403201.215717177] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.

[SteveMacenski]

Do you have enough memory for that size of lookup table (i.e. is this a very small computer)?

[Axel927]

Do you have enough memory for that size of lookup table (i.e. is this a very small computer)?

That was it, thank you.

Might I recommend using a feasible planner like the Smac Planner?

I didn't spend much time configuring it yet, but at first sight, it should do the job.

Thank you for your time.

[SteveMacenski]

No worries, I'm glad we found a solution!