[Smac Planner]: Obstacle Heuristic continue expanding to similarly costed regions

[mangoschorle]

I am experiencing unintuitive results regarding the SMAC Hybrid planner. It appears to me that the planner's exploration is excessively dominated by distance-, rather than obstacle-costs.

I have set up a toy environment, where the distance-wise shortest path is obstructed by a large high-cost (251) area.

I would expect the planner to favor the (distance-wise) slightly longer path alternative path with no high-cost obstacle in between. However, the planner chooses to traverse the high-cost region.

This behavior persists irrespective of the parameterized cost_penalty.

When shifting the starting position slightly (towards the alternative path without the high-cost are) the solution changes.

Issue: The planner produces results, which seem to be heavily biased towards distance rather than costmap-cost. The cost_penalty coefficient has no apparent influence (neither had the analytic_expansion_ratio).

Questions Am I missing an apparent property of the SMAC planner, is this the expected behavior, or may there actually be some issues with the two heuristics. Required Info:

• Operating System:
  • Used the provided docker- development environment on a Ubuntu 20.04 host

Steps to reproduce issue

I have attached the maps and the parameterization parameter_and_map.zip

Expected behavior

Planner balances geometrical distance with costmap-cost and is affected by cost_penalty

Actual behavior

Planner solution is heavily biased towards short solutions

[SteveMacenski]

A couple of things

First see https://github.com/ros-planning/navigation2/issues/2659 which meets the thrust of your ticket. This is known and has a known solution that is being worked on (probably?). Towards the end, it will just plow through higher cost situations if there's a valid solution because that's what analytic expansion means.

Now there are 2 more "actual" clarifications I have to make from your toy situation. This is where a real situation you would see success in but because you've set up an unrealistic setting, you're getting unrepresentative results.

A.

You need to have a potential field in your map. I try to be as explicit as I can about that in the documentation. In fact, all planners in this repo you really should be doing that with, its due to poor historical examples that people just casually inflate and move on. You should always desire to have a smooth potential field so that the planners can steer away from obstacles proactively before running into them in search expansions. While that wouldn't help in some respects, it will significantly speed up the planning process as well as plan in the middle of corridors rather than skirt the edges like your example shows. Nav2 planners are not research planners built to work on binary maps to prove an algorithm, they're production implementations that take into account cost information to be drivable and safe.

B.

The reason your robot chooses one corridor over another has almost nothing to do with that cost zone. Hybrid-A* is a kinematically feasible planner, meaning it generates drivable paths in the presence of real physical constraints. In your 2 examples, the reason it goes down the higher cost hallways is because you've set your initial pose in a way that it is physically impossible to have a drivable path in the other direction. Look at the minimum turning radius in the rest of the path, its not possible for the robot to rotate that tightly to even get to the other hallway because its too narrow and would hit a wall. Therefore the planner has no choice but to go down the higher cost hallway, because the other is entirely unreachable. That's why you see different characteristics based on initial orientation. This example is poor with the settings you've given it.

[SteveMacenski]

I think that fully answers all of the topics possible with this question - if there's more feel free to ask and I'll answer,but closing since this is not an actual bug anywhere and there's no actionable changes for the repo (I believe).

These are good points to clarify, but also if you read over the documentation / papers, I believe these points would be clear to you as well :smile:

[mangoschorle]

@SteveMacenski ,thank you very much for your thorough explanation. The example i provided stems from one of our industrial plants. To get behind the mechanics of the planner I did indeed highly simplify the scenario. The center area (roughly 50mx40m) is where production takes place and vehicles are not granted access. This area is surrounded by a traffic lane, where vehicles may freely navigate.

Concerning your statement in B, I must unfortunately disagree. The starting points in both plans have the same orientation. They only differ in translation (1 meter along the lower path). To take the low-cost path the vehicle must do nothing but drive forwards. To take the upper, high-cost path the vehicle must do a turning maneuver (which it does). I provided close-up images where this behavior becomes more apparent.

Concerning your statement A, I have seen the previous issue #2659. The high cost-area is some 50m away from the goal and starts at around 50% of the path. I didn't consider this to be around the end of the plan.

Anyway, I integrated your advice in my setup. I added very smooth inflations around the obstacles, and made a very smooth transition into the high cost area. I also shifted it further towards the start to not run into the analytic expansion issue. The issues persist. Instead of just driving forward along the low cost route the planner produces a result, where the vehicle actively changes orientation to then plough thorough a smoothly increasing high-cost area.

Shown below is the new setup. Again, the start in the right is just shifted by one meter in translation (to demonstrate the crucial bias towards distance rather than anything the costmap says). In my opinion this cost-neglecting behavior will make future endeavors e.g. working with costmap filters to create preferred lanes ect... extremely cumbersome.

The following is a closeup of the starting-area.

As a side note, but this is a different issue. I observed that the smoother unfortunately seems to destroy the kinematic feasibility in the beginning. The picture below shows this. On the left you have the path before smoothing. It appears to be fine concerning the kinematic constraints. On the right is the smoothed result .

[mangoschorle]

This is the result of Theta*. To me ,it seems to produce more explainable results.

[SteveMacenski]

Ah, I see, the close up images help alot, thanks. That is a serious smoother hiccup, I haven't seen it quite that bad before, that will require some attention. It was a known issue I describe in detail here https://github.com/ros-planning/navigation2/pull/2660#pullrequestreview-800764769 but was told that people really didn't functionally see it so I passed it off.

Now that I understand better, what I think is happening is that our search heuristic is looking for a 2D non-feasible path to the goal that is cost aware to steer the search. Once we're in the expansion phase, we use that and costed-traversal functions to avoid higher cost regions, but if the shortest path through the system itself goes through a higher cost zone than a competing branch solution, then it just does it anyway (but will avoid costs along that branch solution fine, just chooses the wrong branch). In this ticket https://github.com/ros-planning/navigation2/issues/2498 I'm discussing with some other folks about changing this heuristic computation to include more search time in the heuristic to keep going until the graph has been expanded with a solution and exhausting search in parallel branches until the cost is higher than the alternative solution. That would let the search continue to expand down the other hallway to find a solution and potentially use that solution instead to guide the heuristic search.

tl;dr I see 2 things

• Smoother needs to deal with boundary conditions better
• Heuristic expansion needs to keep it up

I think the heuristic item is already pretty much done and we're waiting on @robotechvision to submit a PR (we recently did a big Smac Planner framework re-architecture for State Lattice that was merged a few days ago, so getting back on these smaller issues) so I don't think there's really any action you need to take there. We should have a solution there shortly.

I would however really love your help on improving the boundary conditions for the smoother to make this more reasonable.

Theta* is not going to be feasible, so if that's something important to you (and probably is for your setting), you should stick with Smac and we should be able to get these issues handled in the next few weeks. I would though really appreciate your help on the smoother.

[afrixs]

Yes, this is exactly the two passages scenario I mentioned here so the heuristic admissibility fix should resolve the problem. @mangoschorle, could you check out our fork (branch smac_planner_safe) and test the planner on your map with parameter obstacle_heuristic_admissible: true (or send us your map and nav2 params)?

[mangoschorle]

@afrixs The changes appear to resolve the issue. I am getting very intuitive results. Also the cost_penalty parameter also yields the response one would assume. Fantastic!

Unsurprisingly, the issue with the smoother is still there. It quite consistently, in the beginning of the path invalidates the min turning radius constraint.

I will look into this

[SteveMacenski]

@afrixs thanks! Do you have a potential idea of when you may be able to submit a cleaned up version as a PR?

@mangoschorle thanks for taking a look: note that the reason that you see the discontinuity at the start and end is that we hold the first 2 and last 2 points as constant in the smoothing process, so that we approach the goal at the proper angle. Otherwise, it would mess up the starting and/or goal requested start/end orientations. In the comment I linked above, I think it might be solvable by weighting the constant points in the 5 pt process higher. Certainly, lowering the smoothing weight and increasing tolerances could help, but wouldn't be a codeified solve

[afrixs]

2 months I guess.

To start the process I need to know:

1. do you want to keep your approach there and enable/disable the admissible one using the obstacle_heuristic_admissible parameter?
2. do you want to keep the fast but erroneous travel cost computation there (the one without multiplying obstacle cost by distance travelled) and enable/disable the admissible one using a parameter? If yes, should it be a separate (e.g. travel_cost_admissible) parameter or should obstacle_heuristic_admissible adjust both things?
3. what will be defaults for these parameters?

[SteveMacenski]

With the most recent updates, isn't that mostly nulled out? That's just the setup now after the last PR was merged for all situations.

I think the only thing that needs to be changed now is the queuing changes you made to the heuristic to continue expansion until the cost is the same as the solution cost. The computation of the Heuristic and Travel costs should be admissible now.

Either way, those would be 2 independent changes that we'd want analyzed and merged into 2 PRs so we can get them in independently

Edit: I see you computed the distance heuristic and travel cost slightly differently. This is something also we should consider. The computation of the search expansion method is what fixes this particular issue, if you're OK with it, I'd be happy taking a jump start and pulling that out for use now and we can discuss changing the cost function at a later time when you have cycles. Would that be OK? I'd appreciate your feedback on my PR doing so to make sure it also meets you intent / requirements if you're open to it, but I can do the legwork.

[afrixs]

Oh I see the updated computation, we didn't use it in our admissibility fix, but it's good to speed up the truly admissible search (as described below) or improve quality of the path for the current non-truly-admissible search and I think it will be OK from admissibility point of view. (The term can be optimized into ((i <= 3) ? 1.0 : sqrt_2)*(1.0 + (cost_penalty * cost / 252.0)))

However, what I was talking here (the first two paragraphs) about, is need to update also this line in the same manner, i.e. to NodeHybrid::travel_distance_cost * (1 + motion_table.cost_penalty * normalized_cost)

Just after this change the search becomes truly admissible, behaving, like @mangoschorle called it, intuitively, i.e. finding optimal path with respect to the actual cost penalties given in parameters. However, it will be slower to a certain degree (although I'm curious about how updating the term in heuristic like you did will speed it up), so I would recommend an on/off parameter for it ('on' for safety-critical searches, 'off' for others).

So yes, the update can be divided into 2 PRs,

1. Using the Differential A* 2D search for obstacle heuristic to solve the two different-cost passages problem (and other problems from the same class)
2. Updating the traversal cost term, making the search truly admissible and intuitive. It's just 1 line but needs maybe some speed tests and to add a re-tuned recommended configuration as it changes a lot.

I will create a partially cleaned-up PR for (1.) so that @SteveMacenski can perform speed tests as well and then we can discuss about whether to use an on/off parameter for this part too or if the old code can be fully replaced.

[SteveMacenski]

However, what I was talking here (the first two paragraphs) about, is need to update also this line in the same manner, i.e. to NodeHybrid::travel_distance_cost (1 + motion_table.cost_penalty normalized_cost)

I think that was an oversight on my part. I updated it to include the cost again with my old formulation while I was testing around the change to the H computation. I added the cost there and then moved onto the state lattice admissibility, which is that. It looks like I just didn't go back and update how that was being computed as well.

I would recommend an on/off parameter for it ('on' for safety-critical searches, 'off' for others).

If it makes a big difference, I like that idea. If it doesn't, I'm OK just updating it to use the fully admissible formulation.

I can look at characterizing the topics in (2) tomorrow. I can make quick work of it with either solution.

[SteveMacenski]

I characterized it on some first order tests I've been using - I see that this change is about ~2x slower but I'd need some more testing to really narrow into the specifics. I see that it expands about an order of magnitude more search iterations to get a path that is slightly different, but often not consequentially so.

However, when I kill the penalty functions for non-straight / change, I see the numbers go back down to the prior numbers.

That makes me think that we can make the fully admissible work. I'll just need to retune the defaults... again. This is not something I'm going to get to today, but certainly next week. So no need for the option, we'll just move over to that.

Honestly, penalty free looks pretty good to me. We might just make the defaults for Hybrid-A be penalty free. Lattice needs it a little, but Hybrid-A doesn't seem to need it (though, this is after the smoother).

I'll take care of this point next week. @mangoschorle any thoughts on the smoother?

[SteveMacenski]

So in this I've found that there's a balance that needs to be struck between path quality and run-time using this formulation. After the smoother, things run pretty good actually. However, since long term we are going to move the smoothers into another server, I'm concerned about the raw search-quality path with this formulation where the run-time is in the similar ballpark.

Using the same defaults (change 0.2, non-straight 1.25) the planner is about ~1.5-2.5x slower than it was before. This is where the path quality is about on par. To get performance to about only ~1.2-1.5x slower (what I think is a reasonable trade off using change ~0.0-0.10, non-straight ~1.15-1.2), the path quality is pretty not amazing directly out of search - but perhaps livable. After the basic smoother, its unnoticeable that there's been a change.

Interestingly, in the new formulation the change_penalty looks like the best value is simply disabled. Only non_straight_penalty seems to have a positive impact on path quality w.r.t. time. Example below with parameter change = 0 and non-straight = 1.2 without smoothing. Not amazing, but livable (thoughts?)

So that speaks to me that perhaps having some rough smoothing in the planner's output might be good even if being fed into another smoother later. I'm not in love with that idea -- or I suppose we could just make the smoother server be on by default so that this isn't an issue for most users with the default behavior tree.

I'll submit a PR shortly for you to take a look at and let me know what you think / test

[SteveMacenski]

https://github.com/ros-planning/navigation2/pull/2713

[SteveMacenski]

https://github.com/ros-planning/navigation2/pull/2727 also allows you to turn off the smoother via smooth_path: false so that you can disable it due to current issues. Smoothing was never required, just made things look nicer, especially for the Hybrid-A* planner where the primitives are rather short.

I'm working on a fix, but it won't be a complete fix, it will be a rough fix to improve behavior but still will be capable of generating infeasible smoothing effects. The real smoother described in #2498 from previous work of mine that @afrixs picked up and fixed up will be kinematically feasible.

@afrixs has alot on their plate, so I can't say when we'd have that smoother available, but this should help stem the issue by being able to turn it off.

---

Edit: I may actually be able to fix this, but might take a little while. I think I've enumerated the boundary conditions and edge conditions and I think there's a way to virtually guarantee compliance if I can fit each path segment start/end to a spline or motion model projection that's curvature constrained. That sounds like something that's possible, it just may take me some time to think about how to do that reliably and figure out what techniques I have at my disposal.

For transparency, the issue is in the smoother we create situations at the start / end of paths that there are curvature or orientation constraints being broken. They are also broken when we change directions, but starting with #2442 each direction change is treated like an independent path segment, so that would be intrinsically covered if we cover the boundary condition problem.

I think there are 2 potential solutions

• Use a spline or similar method to fit a function with curvature constraints to the starting pose/derivative along with the fit path, resulting in a transition into the smoothed path from the boundaries
• Use the kinematic model for analytic expansions to fit the start/orientation with the smoothed path

Option A is probably the most principled, but also very difficult to implement and prove will work in all situations. Option B is less principled, but much easier to create with a bit of custom logic. It opens up a few sub-problems

• How do we select the point at which we should model to?
• Should we make use of the original path in this region, or throw it away and make use only the start and this selected point?
• Should we test multiple points and create some metric to use the "best"?

I'm thinking that we should test a few points and select the one that generates the shortest total path length. That would deal with loop-de-loop issues if selecting a point too close to the starting point that is infeasible to get to from the current heading fro smoothing. So its pretty easy to tell when we're too close, but hard to tell when we're too far. I'll need to do a bit of geometry and see if I can't make some principled bounds for points to check. My intuition says it should be something like 1/2 or 3/4 circumference of the circle made up of the turning radius + diameter of the circle since that would represent the longest and most extreme turn possible and a portion of a straightline segment.

[SteveMacenski]

I have a prototype going right now, needs some serious clean up, but it proves the point that this works! I should be able to push a solution before the weekend for you to test @mangoschorle to confirm it resolves your concerns. I'm honestly surprised how effective it is.

Edit: I should have something hopefully by tomorrow afternoon. I have something working now and just doing the last few architectural changes needed to deal with supporting reversing with the changes + making sure we don't introduce any other issues with some smoke testing.

This change should make the path drivable again even after smoothing. I still need to do extensive testing and fix 1 more issue, but its good enough for you to play around with if you're curious https://github.com/ros-planning/navigation2/tree/smoother_feasbility.

There's an "issue" (hard to tell if its an issue at all, really, but I'd like to remove the behavior if I can) where when reversing is permitted that it will take a path that the path planner gave as a circular arc U turn at the start will come out as a reverse a little and then do the circular arc so it can attack the curve at a higher angle. It's actually a pretty neat behavior I like, but certainly isn't what search gave out so I want to suppress that if possible. Besides that, I just need to retune the system, update documentation, and write a few unit tests. I should have a PR sometime tomorrow or Thursday to test.

[SteveMacenski]

https://github.com/ros-planning/navigation2/pull/2731 Please check it out @mangoschorle - makes the smoother feasible again! If this works, I can retarget the ticket only to the obstacle heuristic changes. I'm super happy with these results!

You can see here that we follow curvature constraints, follow directional changes fine for reversing, and the boundary conditions are fully respected. Green is the unsmoothed path, red is the smoothed path. Last thing I need to do is write unit tests and this is good to go from my perspective. Try it out!

---

@afrixs how hard would it be for you to pull out the Obstacle Heuristic changes you made for continuing to search until equal cost is had? I looked over your branch and it seems pretty tied into the costmap downsampler work you had so its difficult for me to do myself and propose a PR for you to approve. The downsampler work is also important, but trying to decouple problems from each other to get incremental progress.

[mangoschorle]

I will check it out and get back to you. Should be no later than beginning of next week.

[mangoschorle]

Here you go! Unsmoothed path: min_turning_radius: 0.7

Smoothed path: min_turning_radius: 0.7 smoother_w_data: 0.2 smoother_w_smooth: 0.3

Smoothed path: min_turning_radius: 0.7 smoother_w_data: 0.5 smoother_w_smooth: 0.2

Don't be confused by the costmap discontinuities. That's just the local costmap above the global one.

[afrixs]

@SteveMacenski I haven't done the downsampler work we were talking about, but something with downsampler was needed to be done for the heuristic to be admissible: use min function instead of max for downsampling (since heuristic cost must be <= real cost) so I had to add a param to choose the function in downsampler.

Currently, as far as I remember, the only extra things in there are the analytic expansion constraints mentioned here + some debug logs/visualizations etc.

Currently I'm working on ceres smoother cleanup/tests + I have some deadlines until 2021/12/17 but I think I will be able to create a PR before Christmas.

[SteveMacenski]

@mangoschorle that looks very good to me, but I do notice that the angle seems a little off in the middle example for the starting heading. I'm not sure why that would be given the solution, it should be rather exact. I'd be interested if you added a print here (or in the End counter part depending if that's the start/end pose) to see if its being overrided or not in that situation. I wonder if the boundary conditions didn't update due to a failure somewhere. I tested on 0.2-0.4 m turning radius. I'll test on some larger ones like you have today. Or this may not actually be an issue and its my eyes playing a trick on myself.

Something that brought to my mind though is that instead of using the starting / ending poses from the path generated by seearch, we can instead have the smoother be passed the starting / ending poses from the planning request so that you actually would have more exact boundary condition matching than would be possible from pure search alone, which is limited by the number of angular quantizations you selected in the parameters. I'm not sure you'd notice much from Hybrid-A* (with defaults at 5 deg increments) but for state lattice, that would make a real difference since that only has 16 bins (22.5 deg, on average).

---

@afrixs thanks for the update. I look forward to it! I'll probably leave that to you then and move onto some other Smac / Nav2 related problems in the meantime, but happy to help if its useful.

[mangoschorle]

@SteveMacenski your "here" isn't very specific about the line number you want your print at. I used 64 angle quantizations. I will add the start and goal from the request to the path and let you know what happens.

[SteveMacenski]

I’ll plan on implementing the start/end from request this afternoon. The “here” prints will show if you expand the smoother.cpp diff, it gets hidden by default because of the size.

[mangoschorle]

Smoother result where I added the start (from request) to the plan. After smoothing, the first point in the result plan has a yaw value of 44.4774 degrees as compared to 0 degrees for the desired start.

In this example i set the smoother iterations to zero, and again, added the first point in the result plan. Interestingly: even though the smoother iterations is set to zero, the first point in the plan has a changed yaw (3 degrees) after smoothing, i.e. the smother alters the first point of the plan even with num_smoother_iterations = 0

[mangoschorle]

@SteveMacenski There is no printing, since if (best_expansion_idx > boundary_expansions.size()) evaluates to true and enforceStartBoundaryConditions returns

[SteveMacenski]

Got it. Sounds like I need to take a look at my skip logic for larger turning radius. That’s good feedback!

Does this always happen for you or just sometimes? Are you able to share with me your inflation settings, map, robot footprint, and start/end goal poses (roughly) so I can reproduce what you see? If not, I’ll try with some of my maps. Your smac settings would be great though.

[mangoschorle]

The points collide hence the function returns no valid index.

Here, in

// Check for collision unsigned int mx, my; costmap->worldToMap(x, y, mx, my); if (costmap->getCost(i) >= INSCRIBED) { expansion.in_collision = true; return; }

You are checking the for loop index.

Are you sure this shouldnt be more like the linear index as inferred from costmap->getIndex(unsigned int mx, unsigned int my)

[mangoschorle]

That did it

[SteveMacenski]

Ahhh! Thanks. I didn't catch that - thanks for looking it over :smile: (this is why peer review is so nice to have)

Updating to costmap->getCost(mx, my)

[SteveMacenski]

FYI - When I change the smoother to use the actual requested start/goal, I see that the smoothed path starts at a different position than the original searched path due to the translation of the global map poses into the discrete grid search space. So this actually makes the paths better, but it does look a little odd if you don't know why that would be happening.

(green is search path, red is smoothed path with kinematically feasible boundary constraints)

[SteveMacenski]

Ah, that actually found the issue. Moving to using the actual planning requested goal/start messes some things up - I'm going to revert that and it solves all of the problems that I had previously described in the comment above containing alot of wrong information. Things are on track.

I have a couple more bugs to work out but 100% solvable -- there's an indexing issue in the reversing logic

Edit: solved. Just need unit testing and retune the gains and this is good to go. Sorry for the delay / mistake.

[SteveMacenski]

Smoother work to be merged shortly. Renaming the ticket to reflect the current scope to be fixed: Obstacle heuristic

[afrixs]

Update: can't make the PR until Christmas, you can expect it around 2022-1-15

[SteveMacenski]

Update: @afrixs has done some awesome work and his PR is close to being merged. We're going to strip out my initial implementation and replace with his and then largely it should be good to go and will resolve this issue.

[mangoschorle]

@SteveMacenski I checked out the new work of @afrixs, played around with the retrospective_penalty and checked the resuts. Great work! I was a bit surprised how sensitive the planning time is w.r.t retrospective_penalty. I halfened my planning time by increasing retrospective_penalty form 0.01 -> 0.03

[SteveMacenski]

Merged and tuned to my satisfaction, should be good to go!