Fix Navigation Goal Rejection in sm_nav2_unit_test_1

[brettpac]

Problem

Navigation goals were being rejected with the error:
[bt_navigator]: Action server is inactive. Rejecting the goal.

The state machine was transitioning from StSetInitialPose to StNavigateToWaypoint1
before the Nav2 stack was fully active.

Root Cause

StSetInitialPose used CbWaitTransform to wait for the map→base_link transform.
This transform became available before the Nav2 lifecycle nodes (especially
bt_navigator) were activated, causing navigation goals to be rejected.

Solution

Replaced CbWaitTransform with CbWaitNav2Nodes in StSetInitialPose.

CbWaitNav2Nodes waits for critical Nav2 nodes to be active:

• PlannerServer
• ControllerServer
• BtNavigator

This ensures the navigation stack is fully ready before attempting navigation.

Files Changed

st_set_initial_pose.hpp

Path: smacc2_sm_reference_library/sm_nav2_unit_test_1/include/.../states/

• Changed include from cb_wait_transform.hpp to cb_wait_nav2_nodes.hpp
• Updated transition table to use CbWaitNav2Nodes events:
  • EvCbSuccess<CbWaitNav2Nodes, OrNavigation> → StNavigateToWaypoint1
  • EvCbFailure<CbWaitNav2Nodes, OrNavigation> → StSetInitialPose (retry)
  • EvMaxRetriesExceeded<CbWaitNav2Nodes, OrNavigation> → StFinalState
• Updated staticConfigure() to use CbWaitNav2Nodes instead of CbWaitTransform
• Updated postEvent call in retry logic to use correct event type
• Updated log messages to reflect waiting for Nav2 nodes

Testing

• State machine now waits for Nav2 nodes to be active before navigating
• Navigation goals are accepted by bt_navigator
• Robot successfully navigates to waypoints