search

ros-navigation / navigation2

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

Revitalization of cost-aware smoother as smoother plugin #2498

Closed robotechvision closed 2 years ago

robotechvision commented 3 years ago

## Feature request

Feature description

Although stated that new cost-aware penalty functions push the plans away from high-cost spaces and feasibility checks are performed, we still find some serious problems with the recently updated approach. Most importantly

Implementation considerations

During our analysis and experiments with the planner we found solutions which bring more consistency, simplicity and time efficiency to the original smoother, due to lack of which it has been deprecated:

Although it may seem like quite a large update of smac planner code that's being requested, whole optimizer can be kept in a separate package/bt plugin, thanks to the flexibility of the navigation2 structure. However, we would like to open the discussion about few minor changes in the smac planner so that the revitalized optimizer could be ported:

Finally we would like to thank you for your work at navigation2 stack. We hope that, although introduced with a pinch of criticism of the recent updates, you will find this feature request not trying to break the current development but giving users an option to step back on a firm stair while the new one is being constructed. Looking forward to the discussion.

Smoothing input (cyan) and result (green): Smoother result Only first 6 meters are smoothed (to improve consistency and time efficiency): Smoother input trimmed

SteveMacenski commented 3 years ago

First things, I'd appreciate it if you sent me your map + the locations to be able to test this myself (parameters you're using too). I suspect a bit of the issue you're really running into is that you've messed with the penalty functions and made it really unbalanced. I definitely don't add guard rails to make sure people don't tune themselves outside of reasonable performance, but so it goes with penality functions - who am I to say the local minima I found is the objective best? My guess is those penalties could be reverted and look a whole lot more like the green line out of the box.

Second off, it doesn't sound like you actually used then the existing path smoother back when it was CG/Ceres based. It really did not work all that well and it took quite a bit of compute time to do very little actual help, that is the primary reason it was adapted to the straight-optimization based solution there is now (which both actually works and is very fast). It basically uses gradient descent with only smoothing terms that checks on collisions to terminate early if necessary. There is no b-splines involved here anywhere, so I'm not sure where you're getting that bit from. But more on smoothing below. As such, I don't think re-adding what was there before was a good idea, but adding something new would be intriguing.

Beyond that point, you're right to say that we probably do underestimate the travel-cost not taking into account costmap-cost (or overestimate the heuristic cost by taking into account costmap-cost, depending on your perspective) but I think you're missing the forest for the trees. Part of the benefit of the smoother is that we can charge closer to the goal somewhat sub-optimally and smooth out the defects that can be created by it. Paired with the analytic expansion also removes a great deal of those artifacts from ever being generated as an algorithm designer. Adding back in the normalized cost term would be the best solution as outlined https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L306-L317, I made no attempt to hide this fact. It would take alot of words to fully describe to you what the obstacle heuristic cost is doing, but suffice it to say that its massively beneficial and having the cost term there makes the planner and smoother's jobs significantly faster and simpler at the expense of a little time up front.

On smoothing, it sounds like you personally made a ton of changes so you were not actually using the proposed smoother which as evidenced by your development. It was an attempt at being as purist to the paper's implementation as possible but ultimately didn't really work all that well in the Ceres framework and took way too long to be usable. My plan was either to rewrite it with a custom optimizer since Ceres was really crapping out at speed or to attempt another strategy. As you can see, I chose another strategy since I modified the obstacle heuristic cost to include cost awareness so that it would steer expansions not only towards the goal, but in the center of of spaces no less. So I felt less need for the smoother to think about steering towards the center of aisles because the plans were already doing that!

If you've made changes you'd like to open source, I'd be happy to consider them! But I made the decisions I did with the best information / time available to me at the time. If that's changed because there's new smoothing options others have developed they want to contribute, I'm stoked to potentially integrate them.

In terms of actionable things here

afrixs commented 3 years ago

path-close-to-corner

Here you can see a basic situation when the planner produces a path which passes very close to the last corner although there is enough space to avoid it from a safer distance. The problem is caused by analytic path expansion which is collision-aware but not cost-aware. That's where the original algorithm relies on the cost-aware optimizer to push the path to a safer zone. You can find the map, parameters, launchfile and rviz file here. The parameters come from nav2_bringup and nav2_smac_planner readme, the only changes are

After installing the package in your workspace (tested with ros2 galactic) and launching ros2 launch planner_playground playground_launch.py you can easily reproduce the situation by calling

ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose "goal:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: map
  pose:
    position:
      x: 8.0
      y: 2.6
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 1.0
      w: 0.0
start:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: map
  pose:
    position:
      x: 6.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
planner_id: ''
use_start: true"

If you asked for data to reproduce the pictures in the request, the aim of the images was to illustrate results of the revitalized ceres path optimizer, not to compare it to the current implementation of smac planner. So the cyan path you can see in there is from our version of smac planner with disabled obstacle heuristics and you are right that the cyan path would probably look better if obstacle heuristics was enabled. (Anyways, if this is what you meant to reproduce, I can provide exact start/goal coordinates for the pictures and information about what to install and launch)

I apologize for using a misleading term, the B-Spline inspired solution is mentioned in nav2_smac_planner readme. The main point was to note that although the straight-optimization based solution is faster, far more consistent, and simpler to understand, the algorithm relies on a cost-aware optimizer (despite the introduction of the new obstacle heuristic), as depicted on the example above.

I agree that having an obstacle heuristic in A planner can definitely be beneficial. I can imagine how a more accurate heuristic leads the A expansion faster towards the goal along a more elegant and safer path. I noticed the note in node_hybrid.cpp and actually wanted to address it in the feature request and point out to the importance of the cost penalty term with respect to A* admissibility requirements on heuristics. Of course the empirical arguments stated in the note are also reasonable so a decision needs to be done whether the planner will pledge itself to follow theoretical requirements or not and we will accept your decision. If decided to follow, I believe it is necessary to reapply the term, although unfortunately I'm afraid it would not be sufficient and further more complex steps would be needed (I have some ideas but this discussion is definitely for another thread). As the resolution of these issues is a long-term task, I wanted to ask whether it would be ok if an option for more theory-conservative users to disable the new obstacle heuristic was added.

On smoothing, yes, there are changes that we have done in the Ceres based smoother. As you say, it was difficult or even impossible to yield good stable results from the implementation present in the planner about 3 months ago. As we had better experiences than this with Ceres from our other projects, we decided to try to improve its performance, while trying to keep the cost computation methods and ideas of the paper unchanged. Although we derogated from the paper in implementation details, I believe that the algorithmic flow remained the same. We simplified usage and tuning by avoiding complicated error-prone jacobian definitions and leveraging on the Ceres auto-differentiation, which is a derogation from the original paper which manually defines them, but didn't show a negative qualitative impact on the optimization. Also we used different minimizer (Levenberg-Marquardt trust region with DenseQR linear solver instead of Nonlinear conjugate gradient) referring (since I'm not an expert in optimizers) to statement of Ceres docs that Line Search Solvers should be used instead of Trust Region Solvers when the problem size is so large that storing and factoring the Jacobian is not feasible or a low accuracy solution is required cheaply, so we decided to use Trust Region Solvers unless they show up malfunctioning or too slow (and they didn't). The paper counts with continuous function of obstacle cost so I believe that usage of bicubic interpolator in the smoother brought it closer to the original paper's implementation.

I will add speed analysis of the smoother tomorrow, I'm sorry for the discussion to be so exhaustive, maybe it would be appropriate to split it up into separate threads.

SteveMacenski commented 3 years ago

I updated the readme, thanks for noting that.

So I don't really need to see any more images of paths if I can't compare them because its based on something you've changed a bunch in the heuristics so its pointless for me to look at. As far as I'm aware there are 2 problems being brought up

On the second point, the solution is to add the costs back into the traversal cost definition. Imagining that is 'poof done' why do you say that there is more? That makes the entire thing admissible again. That should be the end of that topic.

On smoothing, understanding compute time here is really critical. If it takes 500ms, then that's not really an acceptable replacement solution to a planner that by in large takes less than 200ms in total today (can't speak to what metrics you see if you've removed the obstacle heuristic, that one is really important). But it is an acceptable solution, among other solutions, as an option in a more general path smoothing server / plugin interface in planner server / etc.

The old version worked well... until it didn't. Overall, I dumped so much time into it, I just had to ship it because I needed to move on. But I did come back to it and replaced it as soon as I reasonably could get some distance and try again with fresh eyes. The autodifferentiation was intentionally avoided since that is usually super expensive as I learned while using Ceres to make the backbone of SLAM Toolbox. But I'd be interested to see what your results are. On the different solver techniques in Ceres, I did actually test a ton of options and settled down on the 2-3 I exposed as parameters / comments. I think on this type of thing, its worth 'trying and using the best' versus trying to guess based on papers (since Ceres makes it easy to swap around).

robotechvision commented 3 years ago

I will elaborate a bit on the obstacle heuristic. Consider a situation of two possible passages from start to goal. One of them is wide and safe. The other one is narrow, dangerous (although traversable) and cells have high cost. Path through the narrow passage is, however, a little bit shorter. Obstacle heuristic computation, as currently implemented (breadth-first search with a simple queue + readding cells to the queue if their cost is decreased), starts from the goal and try to reach the start node. It goes through both passages by equal speeds (a simple queue is used, not a priority queue) and thus the start node is be reached first by the search, which is then terminated instantly and the high-cost heuristic is returned. It is of course higher than the actual cost of the path from the start node because it would go through the wide low-cost passage and so the heuristic is not admissible. The search would later update the heuristic of the start cell, but the heuristic is wrong at the time of assigning it to the start node. The correct termination criterion should be "terminate when all cells in the queue have higher cost than the start cell", but fulfillment of this condition is hard to detect and additionally the algorithm can have really high time complexity (new search waves arise always when cells are reached from a lower-cost path and cells can be thus recomputed many times. Not actually doing time analysis, but for me the algorithm resembles Floyd–Warshall algorithm and thus maybe has complexity as high as \Theta (|V|^{3}) where V is number of cells in the map. Here is a simple test simulating the problem (although not a real situation with proper inflation etc., hopefully sufficient for illustration). The test would be successful if testB.pose.y = 49;, that's what can also imaginably cause problems with path quality (neighbor nodes with jumps in cost heuristics).

TEST(NodeHybridTest, test_obstacle_heuristic)
{
  nav2_smac_planner::SearchInfo info;
  info.change_penalty = 0.1;
  info.change_reverse_penalty = 0.6;
  info.non_straight_penalty = 1.1;
  info.reverse_penalty = 2.0;
  info.minimum_turning_radius = 8;  // 0.4m/5cm resolution costmap
  info.cost_penalty = 1.7;
  unsigned int size_x = 100;
  unsigned int size_y = 100;
  unsigned int size_theta = 72;

  nav2_smac_planner::NodeHybrid::initMotionModel(
    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);

  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
    100, 100, 0.1, 0.0, 0.0, 0);
  // island in the middle of lethal cost to cross
  for (unsigned int i = 20; i <= 80; ++i) {
    for (unsigned int j = 40; j <= 60; ++j) {
      costmapA->setCost(i, j, 254);
    }
  }
  // path on the right is narrow and thus with high cost
  for (unsigned int i = 20; i <= 80; ++i) {
    for (unsigned int j = 61; j <= 70; ++j) {
      costmapA->setCost(i, j, 250);
    }
  }
  for (unsigned int i = 20; i <= 80; ++i) {
    for (unsigned int j = 71; j < 100; ++j) {
      costmapA->setCost(i, j, 254);
    }
  }
  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72);
  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);

  nav2_smac_planner::NodeHybrid testA(0);
  testA.pose.x = 10;
  testA.pose.y = 50;
  testA.pose.theta = 0;

  nav2_smac_planner::NodeHybrid testB(1);
  testB.pose.x = 90;
  testB.pose.y = 51; //goal is a bit closer to the high-cost passage
  testB.pose.theta = 0;

  // first block the high-cost passage to make sure the cost spreads through the better path
  for (unsigned int j = 61; j <= 70; ++j) {
    costmapA->setCost(50, j, 254);
  }
  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(costmapA, testB.pose.x, testB.pose.y);
  float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(testA.pose, testB.pose);

  // then unblock it to check if cost remains the same
  // (it should, since the unblocked narrow path will have higher cost than the wide one
  //  and thus lower bound of the path cost should be unchanged)
  for (unsigned int j = 61; j <= 70; ++j) {
    costmapA->setCost(50, j, 250);
  }
  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(costmapA, testB.pose.x, testB.pose.y);
  float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(testA.pose, testB.pose);

  printf("wp: %f, tp: %f\n", wide_passage_cost, two_passages_cost);

  EXPECT_EQ(wide_passage_cost, two_passages_cost); // FAILURE

  delete costmapA;
}

I've got some ideas about an approach which could resolve this problem, I'm going to do a little research and share the results. They are difficult to explain and maybe will not work so I apologize for not describing them at the moment.

Also, time analysis of the smoother will have to wait for another day. Just shortly: I agree that the current smoother does its work very well and doesn't need to be replaced. I expect that the results of our proposed smoother will not be sufficient for a 60m path, but plan smoothing can be considered more as a local problem than a global one, so we provide it in a separate node (as putting it in a local planner would also be too restrictive/invasive). Current problem of skipping the smac smoother is only that the issue #2442 is not yet fixed and original orientation information can be lost in the case of Reeds-Shepp movement. However, I agree that resolving the actual issue is more appropriate than adding an option to skip the process (something like if (num_it == 0) { return input; }).

SteveMacenski commented 3 years ago

Please try to keep these comments down to a minimum, I only have so much dedicated time to maintenance and try to keep things concise and to the point. Its very hard to read blocks of text on a time crunch. If you have specific suggestions, please offer them first before a narrative about why. I don't mean to come off short, I'm very excited that there's someone giving some real indepth attention to Smac planner to challenge me on these ideas and have a good conversation, but you did catch me in a few months of timecrunch.

While what you say is not entirely wrong, without this heuristic it is incredibly expensive to operate, that is why the obstacle heuristic was used in the Hybrid-A* planner paper (and mentioned gave a 10x speed up). It has to be used or planning takes on the order of 3-10 seconds rather than 50-200ms -- if only using the dubin/reeds-sheep distance heuristics without any awareness of the environment. The conversation about this is whether or not it includes cost, if cost should be added to the traversal cost estimation, or if there's a way to improve how the obstacle heuristic is being computed (priority queues and other potential ideas were floated) -- which there very well may be.

I do not agree that the heuristic is inadmissible. It might be on a purely distance basis, but that's not what we're measuring in the heuristics or travel cost (if we fixed the travel cost, which I think we've already agreed should be done). We're measuring costed travel over distance. That is admissible and the cost has to be taken into account, not pure distance. Think about it like a gradient descent problem with cost raising the "mountains" and distance is a consistent slope. I can totally understand depending on what part of the code you read first that your perspective would be "distance-based" with this "strange cost inadmissible heuristic" rather than thinking how I think about it with "costed-distance-based" with a "known quirk with a comment in the travel cost removed costmap-cost".

We do requeue cells multiple times and that is important to get a clean potential field to work with for the planner to use to guide search. Without that, the planner path quality drops dramatically. But you're right to say that it does add some compute time, but far, far less than what it saves, even when working with reasonably large maps. A fair critique is that I did not test it on every possible massive space, so there might be edge cases that could be better handled for truly massive spaces, but I did test on 150,000 sqft spaces and still able to plan in under 250ms. But if we do that greedy algorithm in 2D like this heuristic then it allows us to save a boat load of time for the far more expensive search with kinematic feasibility.

But from the above couple of paragraphs, there may be ways to further speed up or improve this computation, and I'd love to discuss those options. I think that is already possible though to basically select either the narrow or wide passages with the penalty functions I offer, though. Because we're not searching in 2D XY space, we're searching in 2D space weighted by cost. That is an important and intentional detail to allow the robot to make "smarter" / safer decisions. The cost penalty allows you to weight how much you'd rather the literal shortest path vs a reasonably short but safer path.

The correct termination criterion should be "terminate when all cells in the queue have higher cost than the start cell"

That is not the correct termination criterion for heuristic search. We use dynamic programming, which again the paper lays out. You terminate when you have a solution to your program and continue to compute as you need additional information. Any heuristic search has no guarantees of optimality. What your suggest there is finding the optimal solution by expanding more after a valid solution is found. Definitionally:

A heuristic search technique is a type of search performed by artificial intelligence (AI) that looks to find a good solution, not necessarily a perfect one, out of the available options.

I think we meet that. Your suggestion isn't heuristic search then anymore, its dijkstras/BFS expansion. That isn't necessarily bad, but wasn't the goal and will take more compute time after a technically correct solution is found. It might be worth exploring, but I think we have bigger fish to fry first.

I'd love to see some metrics on the smoother and see how we can add that too. I think this is an area we can make much faster progress on a fixed time budget. But the obstacle heuristic may have ways it too can be improved, but some of your suggestions are not definitionally correct, though you mention some potential improvements I'd be happy to discuss.

SteveMacenski commented 3 years ago

https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA

I'd like it if you joined the nav2 slack, it might be easier to have this conversation in a more synchronous messenger format so that we can enumerate the points of discussion and talk about them one at a time to keep the thread easier to follow. For now, I see the following topics

Let me know if I missed anything

afrixs commented 3 years ago

Ok, to close the discussion here, I provide the requested metrics (on Intel® Core™ i7-8750H CPU @ 2.20GHz × 12; Gazebo, RViz and some navigation/localization nodes running):

In all of the tests up to now the optimizer (green) performed with no failures and the resulting path was significantly safer than for the current smoother (purple). Tests were performed with parameters provided in the docs (although for a bigger robot, so few parameters were changed footprint: "[[-0.6, -0.35], [-0.6, 0.35], [0.25, 0.35], [0.25, -0.35]]", cost_scaling_factor: 1.6. Screenshot from 2021-08-16 14-36-05 Screenshot from 2021-08-16 15-25-34 Screenshot from 2021-08-16 15-29-50

I will notify you via slack when the nodes will be cleaned up prepared for merging. If you want some custom testing, you can find the nodes here

Briefly on obstacle heuristic to close it too (it's my time and energy spent here as well). The paper is not very verbose on the implementation of the dynamic programming method but I doubt it was the heuristic search implemented, as they state the heuristic to be admissible but this one is not (of course on the travel cost basis). You might not agree but the criteria are objective. A dynamic version of Dijkstra (e.g. D* Lite, possibly without obstacle updates which could be irrelevant here) could do the job and could be fast enough. I'm going to try it out and I will post my results here when done as I see motivations for the planner to be good and compact, of course it's then all upon you, I will not push or persuade you into anything.

SteveMacenski commented 3 years ago

I'm going to reopen this - just because we're going to continue chatting in slack, I think this is still a valuable conversation to keep in the issue tracker until we do come to a final conclusion here.

I think the optimizer you propose would be a good option, though it looks like it over corrects a bit but that is probably tunable behavior. The third picture in particular looks more wobbly than I think most would like given mostly a straight line path - it almost looks like what the planner in its current form would create. Maybe limiting the smoothing to the first N meters would be a decent enough compromise for the heavier algorithm but be aware that some people don't use dynamic replanning if the previous path is still valid so they'd have to do it all in the outset (probably a param for how far or the entire path to smooth).

By the way, I see the smoother-server-esk solutionfor multiple smoother plugins. That was actually my intended long term solution to this, so that would be a welcome addition to Nav2 as a nav2_smoother package with pluginlib interface in nav2_core for smoother that we can implement these options under.

On the obstacle heuristic: I'm happy to have more data and help make better decisions. If you find another way to compute that to take into account more information but doesn't result in too much compute time changes, I'd be happy to consider it for use. So far, this is all from what I've personally built and thought was a good trade off, more opinions and analysis are good. Even if it makes it a bit heavier, it could always be an option set by a parameter if it results in better behavior. I'm willing to work with you on this - but I do want you to push a little on your ideas so we can make sure we're on the same page and can come to some general consensus. If that suggestion is mostly just continuing to expand until you meet the cost threshold of the start node, that sounds very reasonable to me.

as they state the heuristic to be admissible but this one is not (of course on the travel cost basis)

To back up a bit -- if we bring back the costmap-cost element to the travel cost, why do you not feel that would make the system admissible again? Both the travel cost from A->B and heuristic costs from B->Goal would take into account distances and cost. I thought that was something we agreed upon already.

afrixs commented 3 years ago

Yes, the wobbliness can be tuned down by decreasing w_cost param. Problem of the straight line in the third picture is that the path passes very close to the corner of the obstacle. It is a common case in real-world that control deviations occur and in this case it would cause robot to get stuck on the corner or even collide (depending on how footprint_offset is set). These situations were actually our main motivation to give one more chance to the cost-aware smoother.

I will put the nav2_smoother plugin interface to the form as requested and create a pull request.

On admissibility: heuristic is admissible if the cost it estimates to reach the goal is not higher than the lowest possible cost from the current point in the path, i.e. ∀n: h(n) ≤ h*(n). The test above shows a situation where obstacle heuristic of the start node returns value of 92.284286. Assuming the situation is quite simple for non-holonomic motion and so motion penalties will not be very high (let's say it will be 30), let's say that the actual lowest possible cost will be 122.284286. Admissibility is maintained because 92.284286 ≤ 122.284286. Now let's have the same situation but with an additional alternative (although worse) passage. The lowest possible cost from start to goal cannot be higher than before because the path from the original situation still exists. However in this situation obstacle heuristic returns 194.847794. The heuristic is thus inadmissible since 194.847794 > 122.284286. We agreed upon that taking cost to account for travel cost is a required thing to do but not upon that it is the only required thing. In the early discussion I expressed my concerns about this, which were later confirmed by the test. Heuristic doesn't need to be perfect (it's is still just a 2D search) but the admissibility rule should hold for all nodes at all times.

SteveMacenski commented 3 years ago

Agreed on those points, thanks for the info.

Sounds like 2 things then:

Both are in the bullets in my comment above about potential topics for us to chat about on slack (BTW feel free to ping me there too) in addition to the smoother. I think the biggest thing on A / B in this comment is just enabling them and characterizing the changes across a broad set of situations. If its +/- lets say 10%, we can just do it and not worry about parameterizing it. If its more, we might want to make a parameter to let people choose to be explicitly admissible.

SteveMacenski commented 2 years ago

By the way, along with our prior discussion on the admissibility, I've done some testing with State Lattice and it makes a much more pronounced difference there (due to the longer primitives than Hybrid-A*). You were always correct that adding the term to the traversal cost was necessary and there are now places in the code that this now shows a real difference. I had to tweak the formulation a bit more, but we can move forward in that direction.

Rather than distance + normalized and weighted cost, now its distance + distance * normalized and weighted cost, so that primitives longer than 1 unit have their cost "integrated" over the length of the primitive. The normalized cost will be the highest cost traversed as part of the primitive, computed in the collision checker.

This work will be reflected in the state lattice work and merged in there https://github.com/ros-planning/navigation2/pull/2582. It does slow down the Hybrid A which I'm not happy about, but the fact that I can see the difference in State Lattice means there's impact in the Hybrid-A as well, even if its not pronounced. Only drops the Hybrid-A* performance by ~5% after retuning the penalty weights to be proportional, so I think its a worthy trade off. Plus Hybrid and State Lattice share the same obstacle heuristic computation functions, so it would be nice to keep them both identical.

If I end up keeping these changes (likely) I'll need to retune the default penalty functions, since the way they're being applied is different (and the high gains are slowing down the planner without much improvement). I hope to have this all done and merged before the end of the month.

afrixs commented 2 years ago

Exactly, it was my experience in further analysis as well that the main source of inadmissibility was not the obstacle heuristic computation method itself but that every sqrt(2)-nd cell was skipped in the search (for Hybrid planner). Multiplying cost by the distance is a good enough approximation and practically solves the inadmissibility issue for basic cases. In my tests it slowed down Hybrid A* by far more than 5% (computation time changed from 150ms to 7500ms for an 80m long path) but I didn't change default penalty functions (+ some debug/visualisation overhead was present) so it's a good news that you managed to tune it so that it's only 5%!

I think the results are worth the slowdown, because before the update earlier nodes (closer to start) were being highly disadvantaged by the heuristic. That's why when a turn was needed to be done (and the 3D search needed to deflect from the 2D heuristic search lowest-cost path), planner searched only among the latest nodes and used the first feasible turn it found, ignoring the possibility that the turn could have been done earlier more elegantly and in a wider and safer place. So it's a tradeoff of speed (more nodes are expanded - also the earlier ones) for more retrospective in the search.

Anyways, the inconsistent obstacle heuristic computation method can also bring some errors (as shown in discussion above, although not present in basic practical tests) and I managed to find a simple Differential A* search method which resulted in consistent heuristic and was of same speed or even (up to 4x) faster than the original floodfill search in my tests. Not too many code updates were needed to be done, so I'll write here briefly what I changed:

  1. obstacle_heuristic_queue was changed from queue<int> to priority_queue<pair<float, int>> where the priority stands for current cost + euclidean distance to start cell (as we search from goal to start). More exactly, I used unpacked version of priority_queue (i.e. vector<pair<float, int>> + std::make_heap, std::pop_heap, std::push_heap) so that it can be iterated in linear time and thus priorities can be recomputed and queue can be reordered accordingly in O(n) time
  2. whenever a heuristic cost of a cell which hasn't been assigned is requested, euclidean distance to the new start is assigned to the priorities and priority queue is reordered
  3. euclidean distances are being cached (not tested whether the cache is actually beneficial for speedup so maybe not needed)

Reason of the Differential A* heuristic computation being faster in general (in addition to being consistent) is that far less cells are being expanded because the search is guided and every cell is expanded only once. Of course, the expansion itself is slower but in my tests it was at most ~2x slower (queue contained <900 cells for 80m path so the complexity is log(900) = ~10x(iterate, compare, exchange) for pop_heap and push_heap + 8x(compute new indices, check, compute new cost, compare with existing) for expansion).

Additional time complexity is introduced in 2. (queue reordering with O(n) complexity), but this happened at most 100x (in most cases <10x) per 80m search. I can imagine that this could be lowered by a lazy re-prioritization like the one used in D* Lite algorithm but it should be used only for minor changes of priority (when the new requested start cell is close to the previous), otherwise too many cells would be re-prioritized and reordering would become slower than the eager one. However, as the above was just good enough, I didn't try to implement this.

Here are some experimental results along with visualizations (could have been done in a clearer environment, I ran gazebo, ros1_bridge and whole navigation stack so the time variance is quite big):

82m long path from one map corner to another (A-star search guidance didn't have such a significant impact) Original method: 294609 expansions, 22-45ms. Red-colored dots were expanded once, higher rainbow colors multiple times (up to 30x) Our method: 126742 expansions, 24-44ms. All cells were expanded once, higher-than-red rainbow colors indicate fringe reorderings (up to 8x) Screenshot from 2021-08-31 10-26-15

35m long path with goal in the center of the map (much stronger impact of A-star search guidance) Original method: 176079 expansions, 14-24ms. Screenshot from 2021-08-31 11-11-53 Our method: 21623 expansions, 3.8-4.2ms. (fringe reordering happened only once) Screenshot from 2021-08-31 11-06-19

SteveMacenski commented 2 years ago

In my tests it slowed down Hybrid A* by far more than 5%

I saw that too, until I retuned the penalty functions. The reason it pushes so high is that the traversal costs are too high making it explore too many branches. I "turned it off" by setting the penalties to not impact the result and it went right back up in speed. So I'm working today on finding the new good defaults to use to keep the same-ish behavior as before. If you "turn off" the penalty functions with the admissible heuristic, it works "OK" actually but a bit wobbly. Even with the super modest smoothing in the package right now, it's completely removed. I had thought to myself it may be worth just removing the penalty functions altogether if some light 1ms smoothing would fix it up, but figure its still worth keeping in place, just de-emphasizing it significantly. Long term we want to remove the smoother into the server, so keeping the penalties is good in order to have rational path quality out, regardless of external smoothing status.

I managed to find a simple Differential A* search method which resulted in consistent heuristic and was of same speed or even (up to 4x) faster than the original floodfill search in my tests

I would love to see this and/or a PR. Right now we have alot of balls in the air w.r.t. Smac changes so I think a PR would have to wait until the state lattice work is merged, but I'd take a look immediately after. You've clearly been digging deep into it (which you can't imagine how happy I am to have someone look over my work and give me critical feedback) so I would absolutely take any of your work seriously. Always happy to have improvements there.

At some point I did try a priority queue but found it was significantly slower. While having fewer iterations, the speed was lower, and really the important part is speed, not theoretical iteration gains. But you've found something clearly I didn't from my testing. Frankly, it was an afternoon test that didn't pan out and I was already happy with the sub 150ms performance, so I shrugged it off and moved on.

On an update for the state lattice work in https://github.com/ros-planning/navigation2/pull/2582

There's still some more work that will be needed to be done in state lattice after the above, but its enough to merge as the beta for users (would still need support for rotate in place primitive penalty functions and analytic expansions laterally for omnidirectional platforms). That would be the point that I could confidently merge your PRs since I could then have the ability to test your changes against both planners and have my head on straight with fewer variables. My hope is by the end of this week to have all of the above except for docs/tests done, but might bleed a little into next week. I absolutely want this beta in Nav2 by the end of November at the latest.

Keep up the great work, I really appreciate the attention you're giving to this. Let me know if there's things I can help explain.

For your viewing pleasure, this is the result of the state lattice planner, without any smoothing, in 110ms and <10k expansions based on my current level of tuning using an ackermann minimum control set (analog to Hybrid-A). Note: State Lattice's primitives are significantly longer than Hybrid-A, so each expansion covers alot more ground, that's why there are less expansions. But instead, it has way more collision checks per primitive, so it sort of strangely balances out.

image

And Hybrid-A* (no smoothing) 34k expansions in 167ms -- well within norms of the performance prior to the change

image

afrixs commented 2 years ago

Looking forward to the updates and state lattice, the pictures look really good!

At some point I did try a priority queue but found it was significantly slower

It can get slower in some cases but from my experience the A* heuristic guidance makes it usually faster or equally fast. Anyways, it depends on priorities. I can see you have further plans with planner (more frequent replannings, regular checking for better paths in background) so I understand you want to keep the planning duration as low as possible. On the other hand our robots don't hurry anywhere, 80m long path would last at least 160 seconds to be executed so even 1-2 seconds of planning time is negligible if not too many replannings due to new obstacles are expected. The robots just want to avoid problems wherever possible, that's the main reason why we decided to use Smac planner (use of motion constraints and robot footprint inside planner is a strong problem-killer :slightly_smiling_face:) and why I started to dig a little bit into it, requesting options to increase path safety.

I "turned it off" by setting the penalties to not impact the result and it went right back up in speed

This sounds reasonable as every additional motion penalty thickens the search strip a bit. I agree that the penalties should be kept (although de-emphasized) because they can be used not only to decrease wobbliness but also (again the path safety :smile:) to assign higher penalties to turns in high-cost zones than in low-cost zones.

SteveMacenski commented 2 years ago

so even 1-2 seconds of planning time is negligible

Yeah, that would be unacceptably slow to us. I would like to keep this as close to the performance of the planners that users are used to using already (NavFn or similar) so that people can continue to use the same types of strategies but with updated and more robust algorithms. The sub ~150ms planning times we see now is a good minimum to keep and even if we visit cells multiple times, I'm pretty OK with that as long as its faster -- as long as there aren't actual issues with the technique. But what you propose looks reasonable and doesn't add too much time, so I'm OK with this. But I wouldn't be OK with something that would double compute time, for instance. This is the entire reason that heuristic exists -- to significantly speed things up to the goal as opposed to what the pure-Hybrid-A does. There's a reason I dumped so much time into developing Hybrid-A, and it was to make it so fast that there was no compelling reason not to use it in place of other planners.

higher penalties to turns in high-cost zones than in low-cost zones.

Of course, actually I always tested with the cost penalty in place, I was actually only referring to the non-straight and change penalties, but that was imprecise of me

afrixs commented 2 years ago

Of course, actually I always tested with the cost penalty in place, I was actually only referring to the non-straight and change penalties

I actually meant non-straight and change penalties and their attribute of amplifying obstacle cost penalty (i.e. distance*(1 + obstacle_cost)*non_straight_penalty as opposed to distance*(1 + obstacle_cost + non_straight_penalty)) for which they are important for path safety tuning

SteveMacenski commented 2 years ago

Server merged in. State Lattice stuff making the heuristics admissible are also merged in now. It's ready for any follow up PRs + server plugin PR at this point!!

SteveMacenski commented 2 years ago
SteveMacenski commented 2 years ago

@robotechvision Can you take a look at this comment and let me know if your priority queue implementation would fix this issue (not the smoother, the route selection one): https://github.com/ros-planning/navigation2/issues/2690#issuecomment-973890632? What they're saying here is that the robot favors the higher-cost route because its shorter, rather than going down the lower cost but slightly longer one (because the heuristic found a solution first in that direction so steers expansions in that direction).

I recall us having discussions about continuing to search until the cost is higher than the one already found in a different solution, I believe that would solve this problem outright. If that's what your proposition would do with the priority queue, even if it added compute, I'm sold on its value :laughing: .

SteveMacenski commented 2 years ago

I know you're busy with other topics, but I was curious to sync on the cost-aware smoother plugin -- do you have a potential time frame for that PR?

afrixs commented 2 years ago

Being in quarantine right now, I can't work with actual robots, so I've got more time for this. The code is already cleaned up and I have started writing the tests so I think it will be on Monday/Tuesday