getObstacleHeuristic will return negative values

[mangoschorle]

requested_node_cost is negative. requested_node_cost is never overwritten and we reach the return only if requested_node_cost is negative.

I have the feeling it should be

return 2.0 * shared_data_->obstacle_heuristic_lookup_table[start_index];

https://github.com/ros-planning/navigation2/blob/104bdc51fb72e05af4ccb4fb96a2fd421239f016/nav2_smac_planner/src/node_hybrid.cpp#L542

[SteveMacenski]

requested_node_cost is a reference

const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index];

So when the lookup table values are changed in the body of the function, this is updated too. The only way it would continue to be negative is if the cell was unreachable (e.g. disconnected part of the map with no allowable way to get there).

If https://github.com/ros-planning/navigation2/blob/104bdc51fb72e05af4ccb4fb96a2fd421239f016/nav2_smac_planner/src/node_hybrid.cpp#L598 is changed from == 0 to <= 0 that would guarantee that the heuristic is never negative either way.

[SteveMacenski]

#3122 changes that, so no matter what, the H is never negative since the distance H would be computed if the obstacle H was not possible due to non-reachability.

[mangoschorle]

@SteveMacenski My bad! In my code base i search and replaced all const prim_type & with the actual prim_type because I got annoyed with functions that have const bool & as param type. Shouldn't have done that.