[DWB] PathAlign and GoalAlign critics lead to poor obstacle avoidance

[bpwilcox]

The PathAlign and GoalAlign critics appear to bias the dwb_controller path following results to steer the robot dangerously close to obstacles, particularly seen when rounding corners. Removing them (set scale to 0) appears to lead to better performance. There is likely some bug either with the critics themselves or as a consequence of something else that is happening (perhaps the pruning of the goal position?). We should investigate these critics and test that they are being enacted properly.

Steps to reproduce issue

- launch navigation bringup
- set pose
- navigate to goal

Expected behavior

• should reach goal while avoiding obstacles consistently when there is ample space to avoid them

Actual behavior

• moves very close to obstacle posts when rounding corners, often failing with DWB even when the global plan is far from obstacles and there is ample obstacle-free room

[crdelsey]

We need to understand why this is.

[SteveMacenski]

@DLu if you’re interested in touching base here- this ticket would be a natural starting point 😉

[shrijitsingh99]

I encountered a similar issue where the robot would get stuck in sharp turns especially u-turns.
After some digging, it seems as if the robot gets stuck in a sort of local minima as theGoalDistCritic assigns a cost based on euclidean distance from the goal/part of the global plan at the edge of local costmap. So the minimum cost is just going through the wall, not around the obstacle.
Below is a visualization of the GoalDistCritic costs:

So to avoid these minima, cost should be set based on euclidean distance from point on the global path which is at a distance x from current position, where
x = (current_speed * acc_lim_trans_ * (1 / controller_frequency_)) * sim_time_ + minimum_threshold (if TrajectoryGenerator is LimitedAccelGenerator).

Below is the visualization of the modifiedGoalDistCritic costs when the bot is running with speeds of around 0.4m/s:

When the turn comes:

[SteveMacenski]

Shouldn't you just lower the weight on goal distance so it doesn't overpower the others for going around obstacles? It sounds like you have it cranked in one direction really hard.

After some digging, it seems as if the robot gets stuck in a sort of local minima as theGoalDistCritic assigns a cost based on euclidean distance from the goal/part of the global plan at the edge of local costmap

So to avoid these minima, cost should be set based on euclidean distance from point on the global path which is at a distance x from current position, where

These two statements are both very unclear to me what you mean. Can you clarify?

[SteveMacenski]

Also, this doesn't seem related to the ticket above. Is this another thing you're talking about?

[shrijitsingh99]

Shouldn't you just lower the weight on goal distance so it doesn't overpower the others for going around obstacles? It sounds like you have it cranked in one direction really hard.

So if you lower the weight for goal distance the robot gets stuck at its current position as there is no pulling factor driving it towards the goal.

These two statements are both very unclear to me what you mean. Can you clarify?

Essentially the robot gets stuck in the below case as the goal distance is not pulling it in the forward direction instead it is pulling it right.

One solution to this problem is to enable prune_plan but the problem in that is prune_distance is a fixed threshold so if set to too small value the max velocity the robot is limited by it.
My idea was somewhat to dynamically adjust the prune_dist inside the GoalDistCritic based on the robot's current velocity.

Also, this doesn't seem related to the ticket above. Is this another thing you're talking about?

The working of GoalDist and GoalAlign align critic is pretty much the same as the share the same cost grid. So the working of GoalAlign is quite affected by the cost grid generated in GoalDist.
One more way they are related is that setting a small prune_distance will lead to similar behavior as mentioned in OP, because the robot will try to shortcut the global plan by going near obstacles (which yield shorter paths) during turns.

[SteveMacenski]

So if you lower the weight for goal distance the robot gets stuck at its current position as there is no pulling factor driving it towards the goal.

Isn't that path distance critic?

You should 100% be pruning plans unless you have a compelling reason not to. Frankly, I don't know what the specialty reason not to prune even is. If you know, do inform me.

Again, I am not clear what this has to do with the ticket above. Is this related to the PathAlign and GoalAlign critics leading to poor obstacle avoidance performance? I'm not drawing a connection from this discussion.

I'm re-reading your comments and I'm not seeing a question, action item, or otherwise clarification. It looks like you're tuning the gains of these parameters. I'm not sure the behavior you're describing is at all a bug but a tuning problem. Can you please be very clear about what your problem is, what your solving for, and what your solution is.

@DLu can you help at all with this? My familiarity with the specific interactions within DWB is low.

[jackpien]

I am able to reproduce this somewhat deterministically with the following steps:

1. In nav2_params.yaml set:
   1. FollowPath.PathAlign.scale: 32.0
   2. FollowPath.GoalAlign.scale: 24.0
2. Launch the tb3_simulation
3. Set the initial pose:

ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
{
header: {
  stamp: {
    sec: 1588272932,
    nanosec: 798746418,
	},
  frame_id: map
  },
pose: {
  pose: {
    position: {
      x: -2.0232667922973633,
      y: -0.5306379795074463,
      z: 0.0
	  },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
	  }
	},
  covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
  }
}
"

4. Set the goal pose:

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "
{
pose: {
  header: {
    stamp: {
      sec: 1588272932,
      nanosec: 798746418,
	  },
    frame_id: map
    },
  pose: {
    position: {
      x: -0.54,
      y: 0.18,
      z: 0.0
      },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
      }
    }
  }
}
"

---

Running 9 out of 10 times, you should see the TB3 get "jammed" against a pillar, while moving towards the goal, and cause an "No valid trajectories found" error.

---

This "jamming" occurs with either GoalAlign or PathAlign enabled (ie scale != 0.0). Seems to be worse with GoalAlign enabled. With just PathAlign enabled, it "jams" maybe 1 out of 10 runs but otherwise precariously hugs the pillar as it moves successfully towards the goal.

When scoring a trajectory, both GoalAlign and PathAlign forward transform the current pose by a fixed forward_point_distance which defaults to 0.325. Otherwise, the scoring is equivalent to GoalDist and PathDist.

During the initial GoalAlign->prepare(...) there's a transformation for the final goal pose in the truncated global_plan (clipped to be inside the dynamic window?) that looks a little suspect (and I don't really understand the comments in the code). Basically we compute the angle between the current pose and the final goal pose. Then we take the last pose in "global_plan" (which is not really the final goal pose since "global_plan" looks truncated from the "real" global plan - clipped to be within the dynamic window?) - and forward projects that last pose towards that angle by the same forward_point_distance 0.325 meters. This projection can cause the last pose in the "truncated" global plan to be seemingly projected out into an arbitrary location, definitely not guaranteed to be aligned to the goal depending on the current pose. Otherwise, the costmap preparation uses the same code as GoalDist.

---

Things I tried:

1. Setting GoalAlign.forward_point_distance and PathAlign.forward_point_distance to 0.1 which drastically improved the stability, removes the jam I see. A hacky fix for this issue would be to use 0.1 as the forward_point_distance value set in nav2_params.yaml with Goal and PathAlign enabled.

2. In GoalAlign->prepare(...), removed the forward projection of the last pose from the "truncated" global plan and just use the last pose as-is. Seemed to allow the TB3 to follow the global plan a bit more predictably. This seems to be intuitively the correct thing to do. It favors a trajectory that aligns to the final goal pose in the "truncated" global_plan.

---

Things I have not tried:

1. Dive deeper to understand why we get into a "No valid trajectories found" error situation.
2. In GoalAlign->prepare(...) use the angle between the last pose from "global_plan" and the real goal pose to project the last pose.
3. Messing with the 32 and 24.0 scales.

---

Questions I have (given I'm a DWB noob and still trying to understand the nav code in general):

1. How were the 32.0 and 24.0 scale values for the Goal and PathAlign/Dist critics chosen?
2. In the GoalAlign->prepare(...) func, is last pose projection code correct (fwiw, the code is the same as ROS1 locus nav repo)?
3. Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar? Where in the code do trajectories that lead into obstacles get trivially rejected? Maybe the trajectory_generator does this?
4. Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall? Or should the local_planner be able to compensate and still publish safe velocities (although they might not be totally efficient paths)?

Suggestions? Other thoughts?

[SteveMacenski]

Thank you for taking this on and such good setup for reproduction. Can you post a gif of the robot in this "jam" position so there's some visual understanding of what you mean?

@shrijitsingh99 also was looking into DWB behavior, I think he should have some helpful comments on this. @shrijitsingh99 can you comment on the GoalAlign->prepare(...) paragraph? That sounds related to what you were looking into.

A hacky fix for this issue would be to use 0.1 as the forward_point_distance value set in nav2_params.yaml with Goal and PathAlign enabled.

Why is that a hacky fix? Given that this is for just the align critic and not the distance critic / a carrot, that seems reasonable to me. I would be vaguely interested if you used another robot (ex rover since its already in ROS2, usually I'd say Jackal) that's larger if you see the same behavior. Maybe its b/c the TB3 is small?

How were the 32.0 and 24.0 scale values for the Goal and PathAlign/Dist critics chosen?

Heuristically, these are hyper parameters so they're not rooted in math, they're turned by hand to get performance an engineer deems good. Those are probably the params Locus uses if that is the default they set.

In the GoalAlign->prepare(...) func, is last pose projection code correct (fwiw, the code is the same as ROS1 locus nav repo)?

I think you bring up a real potential issue, and the answer is that I'm not sure. Can you post a ticket on the robot_navigation repo and maybe David will answer with your explanation. I would agree at first glance that that should be the real end of path not the end of the window, but I could see that also being the case as part of the "dynamic window" method.

Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar?

I think that's the role of the footprint critic, as its forward projecting if in collision it should score poorly.

Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall

Not 100% sure. Its all a scheduling problem so you could schedule yourself right into a wall if you choose hyper parameters that encourage behavior like that, but I wouldn't think the forward_point_distance would be that parameter, but rather the weights.

[jackpien]

Thanks @SteveMacenski for the thorough feedback.

Maybe its b/c the TB3 is small?

That was my initial thought which is why I adjusted the forward_point_distance down. I figured whatever Locus is using is much larger, at least 2x larger than TB, if not larger.

---

I'll put the following on my things-to-do list:

1. Post a gif in this issue of the "jam" I see with GoalAlign and PathAlign enabled (default forward_point_distance == 0.325).

2. Create a pull request to "fix" this issue by setting forward_point_distance to 0.1 for nav2_params.

   1. I think this is hacky because ideally, I think badly tuned heuristics should cause inefficiencies but it shouldn't cause the system to blow up (ie TB3 to crash into the pillar). The right fix I believe has to do with rejecting any trajectory that will inevitably lead into obstacles given the max acceleration / deceleration parameters of the robot (huh, maybe those parameters in nav2_params are not correct for the TB3??).

3. Will post an issue / ticket in the robot_navigation repo about GoalAlign->prepare(..) to understand what's going on with the goal pose projection - to see if David, etal have feedback on that.

Anything else / other comments (specifically around boldfaced bubble thought in 2-i)? Otherwise stay tuned...

[SteveMacenski]

2i, maybe try that out? I know that in the nav2_params we don't fully describe all the parameters for everything.

I would though like Shrijit's input.

[cdelsey]

Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar? Where in the code do trajectories that lead into obstacles get trivially rejected? Maybe the trajectory_generator does this?

As Steve said, these are rejected by the obstacle critics. Either BaseObstacle or ObstacleFootprint

Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall? Or should the local_planner be able to compensate and still publish safe velocities (although they might not be totally efficient paths)?

In a perfect world, it should be impossible. However, I believe the *Align critcs encourage the robot to take a marginal path, and then either sensor noise or localization noise causes it to think it is on a obstacle. At that point it fails.

There is a topic that DWB publishes - evaluation. This outputs the scores of each critic at every step of the DWB planner. If you can get a rosbag of a failing scenario, you might be able to go back and look at all the scores that led to the failure.

[shrijitsingh99]

During the initial GoalAlign->prepare(...) .. definitely not guaranteed to be aligned to the goal depending on the current pose. Otherwise, the costmap preparation uses the same code as GoalDist.

From what I had understood, it determines the angle of the slope made by the line joining the current position and position. Then it projects the last pose of the pruned global plan using this calculated angle and forward point distance.
So essentially, it serves as a guiding factor in aligning the robots towards the general heading of the goal. If you look at the cost grid, you can see it that the it sort of guides it towards the go.

It's not trying to perfectly align with the goal, it gives a sort of general heading. IMO you should never try to align to the perfect with the goal position unless you are near the goal.

Because of the arbitrary location and its tendency to favor trajectories directed in the heading of the goal even if in that heading there is an obstacle.

Setting GoalAlign.forward_point_distance and PathAlign.forward_point_distance to 0.1

By setting it to a small value you essentially reduce the GoalAlign critic to GoalDist critic so might be why it solves the issue. But I do agree that for turtle bot this value should be the radius of the robot.

One thing you should also do is increase the BaseObstacle scale as GoalAlign is quite similar to GoalDist so by setting both of their scales to '24.0' you have essentially double the scale for GoalAlign so you should also increase the BaseObstale scale., though that might lead to the robot getting stuck (due to sort of minima) but it will not crash.

2. In GoalAlign->prepare(...), removed the forward projection of the last pose from the "truncated" global plan and just use the last pose as-is. It seemed to allow the TB3 to follow the global plan a bit more predictably. This seems to be intuitively the correct thing to do. It favors a trajectory that aligns to the final goal pose in the "truncated" global_plan.

This is not at all trying to align with the goal or even align in the heading of the goal. You are trying to align with the last pose of the truncate global plan, which is not what this plugin intended.

1. Dive deeper to understand why we get into a "No valid trajectories found" error situation.

This is a good idea. Though mostly it is due to the robot crashing or oscillation from what I have noticed.

The right fix I believe has to do with rejecting any trajectory that will inevitably lead into obstacles given the max acceleration / deceleration parameters of the robot
It will always reject such trajectories, the reason it crashes that due to the critics and their scales it drives very close to the robot and due to sensor/localization noise or reaction time of the robot will end up on lethal cost as mentioned by @cdelsey

I think I know the reason behind which is that on turns with obstacles these goal critics and alignment critics essentially cause the robot to not follow the path and try to take a shortcut which
will lead close to the obstacle and causing it to crash. I haven't really been able to explain it properly till now nor have I got the time till now to explore further though I think this is the issue.

The question though remains can this problem be overcome by tuning or is it an issue with the critics. Since @SteveMacenski has mentioned that Locus uses this on their robots and their robots do not face this issue so this is most likely to be a tuning issue.

[SteveMacenski]

It sounds like pushing the .3125 to something smaller is generally agreed to be "good", lets do that. Also another place where explicit documentation is good . . .

@jackpien does that fix most of the problems for the alignment critics or still more to explore?

[jackpien]

Thanks all for the feedback.

Post a gif in this issue of the "jam" I see with GoalAlign and PathAlign enabled (default forward_point_distance == 0.325).

Attached is an image of the "jam" I see when running the repro steps I mentioned above.

---

Create a pull request to "fix" this issue by setting forward_point_distance to 0.1 for nav2_params.

I made a PR that enables the Goal/PathAlign critics and reduces the forward_point_distance for those critics.

In summary to everyone's comment, it sounds like mis-tuned parameters can lead to "jams" due to system noise. But there may be other issues to look at (which I described in the PR comments).

---

Will post an issue / ticket in the robot_navigation repo about GoalAlign->prepare(..) to understand what's going on with the goal pose projection - to see if David, etal have feedback on that.

@shrijitsingh99 it sounds like you think the code is correct. So I will hold off on creating an issue in robot_navigation then.

[naidol]

Hi all,
Not sure if you guys found a solution, but i did now in 2021. use this controller (link below) available in Ros2 Galactic or Foxy.
After much tweaking with the above DWB settings ... i tried this controller below ... it works perfect for my 4wd robot (rectangular shape - long wheel base).

https://index.ros.org/p/nav2_regulated_pure_pursuit_controller/#galactic

just cut and paste the to replace all the DWB controller nonsense above.