Fix missing cost consideration in first expansion of SmacPlannerHybrid

[wang-fire]

…d (#5626)

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Closes #5626
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	Hardware robot
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

This PR fixes an issue where the first node expansion in SmacPlannerHybrid
incorrectly returned a constant cost when the parent node was the first node.
This caused inaccurate travel cost computation.

---

Description of documentation updates required from your changes

No documentation updates are required since this change only affects internal cost computation logic.

---

Description of how this change was tested

After removing the first node condition, I tested multiple scenarios:

1. Goal point in front of the robot — normal behavior.
2. Goal point behind the robot — now behaves correctly (no unnecessary reverse).
3. Goal and start at the same pose — generates a single-point path without issues.

---

Future work that may be required in bullet points

Considering that getTravelCost might be used in other scenarios in the future,
it would be safer to add a check to verify whether the child node is nullptr.
If it is, we can return a constant cost.
This would prevent potential crashes in situations where a nullptr child node
might appear during expansion.

---

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

This pull request is in conflict. Could you fix it @wang-fire?

[mergify]

@wang-fire, 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).

[wang-fire]

Hello, @SteveMacenski, i can't pass the test, even though I added a check to ensure that child is not equal to nullptr, and I encountered a segmentation fault. I suspect it might be due to an array out-of-bounds issue, and the possible of suspicion is here:
float travel_cost_raw = motion_table.travel_costs[child->getMotionPrimitiveIndex()];
I need some time to check if there are scenarios where child->getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max() and getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max().

[SteveMacenski]

See Mat's comment in the ticket please! He has a good point about another necessary change

[SteveMacenski]

Check CI, this also causes now a segfault