IsPathValid Improvement: Change in Cost

[SteveMacenski]

Have an option to have the paths return their path costs and be able to use that with a replan to compare if the cost have changed "sufficiently" to a parameterized setting to trigger a replan, rather than only when exactly invalid due to collision.

Allows for paths that have become much more expensive to be replanned based on a user-defined metric

Issues to work through:

• If we have the old path and the new path is shorter since it was replanned after some distance was traveled, how to compare these paths "fairly"?
• Probably involves storing each point-costs in the path and using only the still relevant costs
• That would probably involve a change to the ComputePathToPose API
• Where to store this information? In the planner server? That would make it stateful. If the BT navigator? Probably the best option but where? Probably the blackboard
• What's the metric we want to use? I would suggest a percentage change potentially.

[SteveMacenski]

@jwallace42 might be a good next step for your is path valid work

[jwallace42]

So, we want to expand the isPathValid to also replan if the cost of the path has changed significantly.

So we would have to store a map of points to costs.

• If we have the old path and the new path is shorter since it was replanned after some distance was traveled, how to compare these paths "fairly"?

Why would we have created two paths? I figured we would be checking to see if the current path costs have changed?

I think the plan would be to compare the path cost subsection. So I have point 1,2,3,4,5 with costs 20, 35, 40, 20, 64.

As the robot moves our new closest point is 3 and our new costs would be 3,4,5 with new costs of 52, 60, 21.

Then we could compare the old 3,4,5 costs to the new and we don't have to worry about the difference in path length. A average of each path cost could be taken and checked against a percentage chance.

• Probably involves storing each point-costs in the path and using only the still relevant costs

I agree!

• That would probably involve a change to the ComputePathToPose API

Just one new field for the percentage change that should trigger replanning.

• Where to store this information? In the planner server? That would make it stateful. If the BT navigator? Probably the best option but where? Probably the blackboard

Are you talking about the map of points to costs?

• What's the metric we want to use? I would suggest a percentage change potentially.

I think so but I think we might need to be a little clever. If we have a average path cost of 10 and it changes 30.(300% increase) should we replan? I don't really think so. I think we could have f(average_cost) = percent_diff to trigger replanning.
That way at low cost changes we don't care as much but at high cost changes we care more about changes to the average cost.

[SteveMacenski]

Why would we have created two paths? I figured we would be checking to see if the current path costs have changed?

... I'm mixing up 2 completely different trains of thoughts in my head. No, you are right, we don't need 2 paths. Just the one that we're checking its change in cost over time. That was for a completely different idea that is largely moot now.

Just one new field for the percentage change that should trigger replanning.

Maybe also have the path planner return a vector of costs so that someone could conceptually use them in the BT or other application?

Are you talking about the map of points to costs?

Yes

I'm not sure I understand you f(average_cost) = percent_diff example, can you elaborate?

[jwallace42]

Maybe also have the path planner return a vector of costs so that someone could conceptually use them in the BT or other application?

So, we could have to adjust the api for the computePathToPose to include a vector of costs.
Then in the BT we could dump the vector of costs on the blackboard to be used by whatever.
Then the isPathValid service call would include the path and vector of costs to be evaluated.

I'm not sure I understand you f(average_cost) = percent_diff example, can you elaborate?

Sure. So for example we could have a linear func cost_diff_to_cause_replan = -0.4*(average_cost-255). I guess the main idea is that we would want try and replan more often for changes in higher cost paths.(Maybe this does not make sense, let me know:)).

Continuing of the metric for determining when to replan, I don't think a percentage change would work well. We would want a cost change right? Any percentage calculation will cause more replanting at low costs and less at high costs.

[SteveMacenski]

What about adding it to the nav_msgs/Path itself? Add a float32[] costs field so that its embedded, I think that seems pretty reasonable and has reuse across a number of applications where there are weights / costs associated with poses in the path.

But certainly putting it in computePathToPose is reasonable too, but I think semantically it "belongs" as an attribute to the path.

[SteveMacenski]

I guess the main idea is that we would want try and replan more often for changes in higher cost paths

that's interesting, I think the idea just needs to be flushed out a little more. I think you're on the right path. minor changes in inflation aren't nearly as interesting as changes close to obstacles.

Note that the inflation costs are an expontential decay function from the obstacles. I'm not sure we necessarily need to use that relationship, but its good to be aware of when designing this. There is some built in 'costs change more rapidly' closer to obstacles. But obviously if its already very close to obstacles, there's only so much higher it can get.

[jwallace42]

What about adding it to the nav_msgs/Path itself? Add a float32[] costs field so that its embedded, I think that seems pretty reasonable and has reuse across a number of applications where there are weights / costs associated with poses in the path.

But certainly putting it in computePathToPose is reasonable too, but I think semantically it "belongs" as an attribute to the path.

Yeah, adding it to the nav_msgs/Path is the right approach. I assume I can just put in a pr https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Path.msg?

This also involves some changes to the createPath method in the planner plugins to populate the costs.

that's interesting, I think the idea just needs to be flushed out a little more. I think you're on the right path. minor changes in inflation aren't nearly as interesting as changes close to obstacles.

Note that the inflation costs are an expontential decay function from the obstacles. I'm not sure we necessarily need to use that relationship, but its good to be aware of when designing this. There is some built in 'costs change more rapidly' closer to obstacles. But obviously if its already very close to obstacles, there's only so much higher it can get.

I will probably start just with a constant change to trigger replanning. Depending on how that works we can always make it more fun!

[SteveMacenski]

Why close the PR?

I think it is worth discussing the math and coming up with a solution in the front-end since we have an intuition that its going to not necessarily work out as well as we'd like.

For instance, I was thinking that if we have a long path, lets say its straight and then does a 90 degree turn and then more straight. If the area around the turn actually has shelf that's moved a little, then the turning arc will have increased cost relative to its initial path planning solution. However, the straight lines will be fine.

In that situation, we'd want to replan potentially because that small region has sufficiently high change to need replanning in that local region, but when you add the costs over the full path, its not going to be a big percentage differential as a small proportion of the path.

Different from your initial idea, but I think this has a similar ring to it. I think rather than naively analyzing the path cost as a whole, maybe we should march through segments of the path and analyze them individually if a path is over N meters in length?

Or, we have weights on the costs assigned to weight more heavily points that are in increased cost areas vs points that are in low cost areas changing. That would scale changes in higher cost areas more than in lower cost areas. We could even threshold the weights to 0 if the cost changes in low cost regions are below a threshold (e.g. if below 50 in absolute cost, probably configurable, ignore any change) to help remove the contribution of low cost area changes.

That would let us ignore the straight line segments if in low-cost space and focus in on the turn's changes if going into higher cost areas.

[jwallace42]

Sorry about the premature close. I saw your email late and though that it would be a discussion instead of a pr.

For instance, I was thinking that if we have a long path, lets say its straight and then does a 90 degree turn and then more straight. If the area around the turn actually has shelf that's moved a little, then the turning arc will have increased cost relative to its initial path planning solution. However, the straight lines will be fine.

In that situation, we'd want to replan potentially because that small region has sufficiently high change to need replanning in that local region, but when you add the costs over the full path, its not going to be a big percentage differential as a small proportion of the path.

This is a very good point. So averaging should be tossed out.

Or, we have weights on the costs assigned to weight more heavily points that are in increased cost areas vs points that are in low cost areas changing. That would scale changes in higher cost areas more than in lower cost areas. We could even threshold the weights to 0 if the cost changes in low cost regions are below a threshold (e.g. if below 50 in absolute cost, probably configurable, ignore any change) to help remove the contribution of low cost area changes.

I like this idea! What are you thinking of for a formula?

[SteveMacenski]

I mean I'm just spit balling ideas still, I'd be open for other formulations or thoughts. I have no idea how well this would work, this is just a starting point to see how it works / what problems it doesn't solve and go from there. If we start with cost as the base term, we could have a weight be something like w_cost = cost > 50 ? f(cost) : 0 where f could be something along the lines of f(cost) = cost / cost_max_valid to be normalized between 0 and 1. Then sum across the path. But if we did something like this, f(cost) is the real function we want to find (the rest is just thresholding). There are many such formulations, including weighting the error between the old and new costs at that point, but that would impact lower-cost terms as well (though if we thresholded, maybe not as much of a concern).

Another thing if we could try exponential, rather than having them be linearly added, they could have the cost^2. I'm not sure what we'd do for the comparison point though. Percentage doesn't seem as meaningful in that situation, it might have a similar effect but wouldn't be very principled.

[jwallace42]

Yeah, I will have to think about this over the weekend! I am still unsure of how we want to compare costs. I will try to come up with something for Monday.

[SteveMacenski]

Hi! Any update / thoughts to share?

[jwallace42]

I have some though but not much progress. I am still stuck on the override.

I was taking with Tom about partial replanning.

The idea is if a section of the path has a significant cost increase we only want to replan that section. This idea is kind of similar to http://www.cs.cmu.edu/~ggordon/mlikhach-ggordon-thrun.ara-tr.pdf. The end result would more deliberate movement from the robot.

[SteveMacenski]

I'm not sure that's a good idea if we're aiming to make this generic and not as part of a particular planning algorithm. The change in cost might make it more beneficial to go down an entirely different route due to a blockage or other environmental changes so replanning just a small section of it would, in some situations, make that impossible or result in super strange paths depending on how we define the "start" and "end" of the partial replanning segments (ex. "start" of change is at the end of the aisle and "end" of change is around the corner from it, so you want to replan around the corner leaving an aisle. However, the change is due to a blockage, so your new path would still go all the way down the aisle, just to come back up to go around. If you replanned from scratch, you'd just immediately turn yourself around).

I think its better to stick with just having this be a trigger for a requested replan and we can let the specific planning algorithm decide on what they would like to do with the replan request information. You could always still have a repairing algorithm implementation that would do this automatically without us needing to model it in the IsPathValid function (e.g. replan comes in, it itself will see that there's a change in a particular segment to try to repair).

So I think what you're advocating for is another planning option or an addition to the existing algorithms - which isn't a bad idea - but not quite what we're aiming for in this ticket 😄 I'm not sure off hand how hard it would be to change the A* in Smac to be repairing, but that could be something to consider offering. Though, I think of "repairing" planning algorithms core assumption that the environment doesn't change much, which is typically not how I think about robotics. But a parameterized option would be totally acceptable and probably of interest to many folks.

[SteveMacenski]

So I did some noodling on this and I think something worth trying is a T-test in probability theory. If we have the average costs of the initial path plan, the new average cost, and can compute the variance of the current path (we can do), given the number of path points, we can compute a t-score and check within confidence intervals if the change is statistically significant.

With that said, I have pretty bad intuition on probability theory so I'll need to do some tests with real examples and numbers to see if that overcomes the problems we were interested in w.r.t. localized changes. Large but localized spikes I think should be handled by the std computation, but I'm not sure to what degree. I'm also pretty sure this won't handle high cost becoming higher cost situations (e.g. 210 -> 230) as much as lower cost -> higher cost situations. Though I do wonder how much that functionally matters considering the planners should be trying to go into lower cost areas, whenever possible. If we're already at 210 and we go to 230 (but still valid) that might only actually be the difference in a pixel or two.

Another direction I thought about was squared loss functions, e.g. X = sum(cost_initial_i - cost_i)^2 probably also normalized. That would make large changes in cost really stick out and small changes not so much. I'm just not sure what the metric it is that we would use to say if the change is worth changing. But while things are "similar" it should be near zero, then spike quickly on changes. If we added a weight term based on current cost, that would also highly penalize changes in high cost settings. That formulation is also the same as sample variance in probability theory (which is what got me down the t-test direction) so there's maybe something of interest otherwise we can take into account from that. What's nice is that we can think of each path point as independent for this analysis, so it does lend itself well to probablistic thinking.

More hacky, but we could also use a hyperparameter value N for a sliding window size to check points within, and if there's a change > X% within the window, we retrigger a plan.

I think there are 3 fundamental situations we want to know if happens (please let me know if there are any others I missed, its important)

• High costs go higher, e.g. we're rounding a corner and our localization was off so the path is now closer to a wall/object than comfortable
• Low costs go higher localized, e.g. we're in largely free space and something gets in the way or nearby like a person so in a small patch of the path, the cost is rising rapidly
• Low costs go high regionalized, e.g. something big is changing in a non-trivial part of the path

[jwallace42]

I don't think you missed any fundamental situations. Once we get some data implementing any of these solutions should be trivial. Hopefully, I can get something up and running this next week.

[SteveMacenski]

I did some rough estimations and if using a 5% t-test (look up table https://www.sjsu.edu/faculty/gerstman/StatPrimer/t-table.pdf), it was able to pick up on a localized increase representing 15% of the points in a path having a pretty big spike in cost (e.g. 3/20 points I set values between 150 and 200 while the rest were 50).

I'd even argue that actually a 5% test is too lenient, I'm seeing with only 8/100 points increased to 100 from 50 being enough to trigger a change.

[SteveMacenski]

I did a much deeper dive into the statistics we might be interested in and came up with the following

• We may prefer the Z-test or modified Z-test over the t-test but I need to think about the ramifications more
• We probably care about a single tailed test, though a 2 tail statistical test would tell us (when negative) if the costs have actually dropped significantly to be worth a replan due to changes that might be more optimal. So we should decide if we only want to retrigger planning due to the negative situation (things cost more, more danger) or also the positive situation (costs are dropping around us, maybe there's a better option). I think for short paths this would be very prone to error.
• We need to find the minimum number of points to even consider non-collision based replanning, below a certain number of points, anything in statistics is going to be really sensitive. For Z-test that's ~30.
• While localized sliding windows for percentage changes is hacky, localized windows for computing probabilities might be useful to model localized changes -- but I don't think it'll be necessary. This comes from anomaly detection literature
• Something I was toying around with but couldn't find any examples of online (or my google-foo skills stink) was creating a histogram of cost values and then having the inputs to a probability model be the values of the number of items in each bin. That probably doesn't make any sense, so what does actually make sense would be to use the static path planning histogram and compare that to the current iteration's histogram, paying attention to the growth trends in higher cost histogram bins. (https://stats.stackexchange.com/questions/7400/how-to-assess-the-similarity-of-two-histograms)

[jwallace42]

I took another stab at running the updated path message with the nav2 stack. Unfortunately, I was not able to make any process.
I posted a question in ros answers.

[SteveMacenski]

Try to make it as non-navigation specific as possible (.e.g updated Path message and here's the error I'm seeing while trying to use it, here's what im rebuilding from source, etc) to get the best chance someone at OR will respond

[jwallace42]

Okay,

I when though the steps to reproduce the error in the question so it hopefully is separate enough...

If I don't get a response in a couple of days I will try again.

[SteveMacenski]

OK a few specific things to try (mostly to summarize my findings in 1 place)

Rules at apply

• Must have > 30 path points, else pass. For a 5cm resolution, that's a 1.5m path which should be so close to the goal anyway that this isn't super relevant. For a 10cm path that's 3m which is borderline, but still I think a reasonable assumption for our use-cases and the statistical tests
• Applying a single tail distribution, we only care about the negative case of outliers (e.g. cost has risen 'significantly') not the positive case (e.g. costs have dropped 'significantly'), although, we could make it a parameterized option to care about both.

Tests to try

• Use the 2 sample Z-test on the path point costs as the data (though may also be worth trying one sample for kicks, though I think its wrong) -- links to equations at a 95% confidence level (e.g. alpha .025)
• Create a histogram with N bins evenly distributed, and count the number of path point costs in each bin. Then, apply the Chi squared test against the original path's distribution to see if it is statistically different

Written explanations in the code

• What the null hypothesis and why we use the statistical metrics / confidence intervals we do (and if should be exposed as parameters)
• Justify minimum number of points and distribution tailedness

[jwallace42]

So, I haven't gotten any response from my questions on ros answers. Is there someone you know that might be able to point me in the right direction?

[SteveMacenski]

What question? Can you link here?

[jwallace42]

https://answers.ros.org/question/400228/overlaying-nav_msgs/

[SteveMacenski]

I think this might be time for the nuclear situation. I think you may need to do a full ROS 2 source build & before you build it, checkout out the interfaces branch with the updated nav_msgs so everything is built from scratch with that as the only thing in the path. The key here is to build it without sourcing anything in /opt/ros in any of the terminals. That way your version is the only version anything knows about. We used to have to do that often in the early days of ROS 2 before binaries were released.

It will take a few hours to run, so my recommendation would be to set up the build after work hours and check on it once before bed just to make sure it didn't crash / die from a missing dependency.

[SteveMacenski]

Use vcstools to get everything here: https://github.com/ros2/ros2/blob/master/ros2.repos & then here: https://github.com/ros-planning/navigation2/blob/main/tools/underlay.repos to get everything for Nav2.

I'd recommend doing it in 2 workspaces. Do the ROS 2 core stuff in that first file in 1. Then once that is done, source it (do not source /opt/ros ever in these terminals) and then build the Nav2 underlay. Then once that is done, source the underlay and then build nav2 in its own workspace. That will allow you to build ROS 2 which should be stable without any issues. Then the underlay in case there is an issue it doesn't kill the compilation of the really long stuff in ROS 2 core. Then when developing you only need to rebuild the workspace with nav2 alone.

[jwallace42]

Sounds good!

[SteveMacenski]

I'm in the middle of a full ros2 core build for #3014 so I'll plan to play around a bit with this too and prototype. The build is actually not taking that long if you remove the cyclone and iceoryx from the build (since we are going to just use the default RMW for testing). Probably will take about 2 hours so might be good to have going in the background of another task or left overnight.

I'll plan on prototyping something and I'll give you a link to the branch, not sure how far I'll get in testing beyond just coding up an example statistical solution as a good starting point.

[SteveMacenski]

https://github.com/ros-planning/navigation2/tree/is_path_valid_cost_change

There are a couple of TODOs around the (1) critical region to use for the Z test, I think we need to play with that to see what makes most sense. 90, 95, and 99% are pretty typical numbers. We also might want to (2) improve the way we find the closest path point, but not critical right now.

It would be good to have some experiments to run with this BT node where we give it some paths and mess with the cost fields in gazebo with obstacles to see when we can get it to trigger. Some example experiments to test this method's effectiveness and maybe to tune a bit (or try other methods)

• Create some paths and find how much change before triggers
• Check for long paths that a localized change of a 1x1m block can trigger a replan. Maybe also try something smaller representing a person or small robot
• check for paths rounding corners doesn't over trigger due to sensor noise -- especially in cluttered environments like warehouses where there are presumably a number of turns

Either way, play around with this and let me know what you think!

[jwallace42]

Sounds good!

Since the assisted_teleop work is wrapped up, I will be building ros2 this weekend :).

[SteveMacenski]

Consider also using E-graphs to keep paths in the similar path solutions or strategically replanning and making the choice at re-planning time about whether the new plan should be forwarded on