Fix missing cost consideration in first expansion of SmacPlannerHybri…

[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-*.

[SteveMacenski]

@wang-fire check out the failing CI / linting jobs to fix the issues

Sorry for the delay, I was at ROSCon and then a short vacation

[SteveMacenski]

I think you should implement this as Matt suggested in

Otherwise we could move the first node check to https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L418 so that we don't apply any turning or change penalties on the first expansion.