Fixing type conversion issues in Smac Planner changes

[SteveMacenski]

May resolve Issue 3 in #5192 @slazarev8 please test!

CC @stevedanomodolor

[mergify]

@SteveMacenski, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

See #5221 and #5222 -- closing and to be included in a regression fix PR

[slazarev8]

May resolve Issue 3 in #5192 @slazarev8 please test!

CC @stevedanomodolor

Sorry for the late reply, but it did not resolve issue 3. However, I see that you have already reverted the entire PR.

I've been trying to debug the issue for some time, and here is what I found:

1. At the finish point, obstacle_heuristic is 0.00001 (which looks like an uninitialized value) — this is strange. Hence, NodeLattice::getDistanceHeuristic returns 0.0.
2. As a result, A* finishes at this point:

if (approach_iterations >= getOnApproachMaxIterations()) {
       return _graph.at(_best_heuristic_node.second).backtracePath(path);
     }

But I still have no idea how this is connected to the PR, and why I can't reproduce the issue after reverting it.

[stevedanomodolor]

@slazarev8 Really appreciate you testing this. We were able to find the issue. It look like I was managing incorrectly pointer. It happened in an edge case when cache_obstacle_heuristic was set to true.

Can you test this branch https://github.com/stevedanomodolor/navigation2/tree/fix/smac_planner_orientation_goals. This should include the fix for the three problem. I am still testing it to make sure I am not missing something.

The problem was bas pointer handling.

Again thanks you for bug you found.

[slazarev8]

Can you test this branch https://github.com/stevedanomodolor/navigation2/tree/fix/smac_planner_orientation_goals. This should include the fix for the three problem. I am still testing it to make sure I am not missing something.

Yes, this one fixes my issue

It look like I was managing incorrectly pointer

@stevedanomodolor Could you, please, provide more details which pointer was the issue?

[stevedanomodolor]

Can you test this branch https://github.com/stevedanomodolor/navigation2/tree/fix/smac_planner_orientation_goals. This should include the fix for the three problem. I am still testing it to make sure I am not missing something.

Yes, this one fixes my issue

It look like I was managing incorrectly pointer

@stevedanomodolor Could you, please, provide more details which pointer was the issue?

The long explanation is

The issue occurs when cache_obstacle_heuristic is set to true. By default, it's false, which is why I wasn't able to reproduce the problem initially.
When caching is enabled, the code enters the "goal has changed" logic. The idea of the code which i missed here is that addtograph reuses the same pointers in between goals to be computationally efficient.
The logic step i use when i assign the previous goal to the current goal is wrong because since the goal in the goal manager and the goal generated from the addtograph function uses the same pointer, when i create a new goal with a new value, i change the goal values stored in goal manager.
As a result, I'm effectively assigning the previous goal to be the current goal again. This means that on the second attempt, we never detect a goal change, and therefore never reset the cache. In addition the start goal and the end😬.

 auto goalHasChanged = [&]() -> bool {
   if (previous_goals.empty()) {
     std::cout << "No previous goals, assuming goal has changed." << std::endl;
     return true;
   }
 
   const auto & prev = previous_goals.front().goal;
   const auto & curr = goals_state.front().goal;
   return (prev->pose.x != curr->pose.x) || (prev->pose.y != curr->pose.y);
 };
 
 // Goal has changed — reset the obstacle heuristic if needed
 std::cout << "Checking if goal has changed..." << std::endl;
 std::cout << goalHasChanged() << std::endl;
 
 if (!_search_info.cache_obstacle_heuristic || goalHasChanged()) {
   std::cout << "Resetting obstacle heuristic..." << std::endl;
   if (!_start) {
     throw std::runtime_error("Start must be set before goal.");
   }
 }

Which makes sense why ithe first iteration works and then it stops working.