Fix Navigation Goal Rejection in sm_nav2_unit_test_1

smacc2/include/smacc2/client_core_components/cp_action_client.hpp

@@ -277,9 +277,10 @@ class CpActionClient : public smacc2::ISmaccComponent
   }
 
   template <typename EvType>
-  void postResultEvent(const WrappedResult & /* result */)
+  void postResultEvent(const WrappedResult & result)
   {
     auto * ev = new EvType();
+    ev->resultMessage = result;
     RCLCPP_INFO(
       getLogger(), "[%s] Posting event: %s", this->getName().c_str(),
       smacc2::demangleSymbol(typeid(ev).name()).c_str());

smacc2/include/smacc2/smacc_default_events.hpp

@@ -47,13 +47,13 @@ struct EvActionFeedback : sc::event<EvActionFeedback<ActionFeedback, TOrthogonal
 template <typename TSource, typename TOrthogonal>
 struct EvActionResult : sc::event<EvActionResult<TSource, TOrthogonal>>
 {
-  // typename TSource::WrappedResult resultMessage;
+  typename TSource::WrappedResult resultMessage;
 };
 
 template <typename TSource, typename TOrthogonal>
 struct EvActionSucceeded : sc::event<EvActionSucceeded<TSource, TOrthogonal>>
 {
-  // typename TSource::WrappedResult resultMessage;
+  typename TSource::WrappedResult resultMessage;
 
   static std::string getEventLabel()
   {
@@ -71,7 +71,7 @@ struct EvActionSucceeded : sc::event<EvActionSucceeded<TSource, TOrthogonal>>
 template <typename TSource, typename TOrthogonal>
 struct EvActionAborted : sc::event<EvActionAborted<TSource, TOrthogonal>>
 {
-  // typename TSource::WrappedResult resultMessage;
+  typename TSource::WrappedResult resultMessage;
 
   static std::string getEventLabel()
   {
@@ -89,7 +89,7 @@ struct EvActionAborted : sc::event<EvActionAborted<TSource, TOrthogonal>>
 template <typename TSource, typename TOrthogonal>
 struct EvActionCancelled : sc::event<EvActionCancelled<TSource, TOrthogonal>>
 {
-  //typename TSource::WrappedResult resultMessage;
+  typename TSource::WrappedResult resultMessage;
 
   static std::string getEventLabel()
   {

smacc2_client_library/cl_nav2z/include/cl_nav2z/cl_nav2z.hpp

@@ -27,6 +27,12 @@ namespace cl_nav2z
 class ClNav2Z : public smacc2::ISmaccClient
 {
 public:
+  // Type aliases required for event system (TSource::WrappedResult in smacc_default_events.hpp)
+  using ActionType = nav2_msgs::action::NavigateToPose;
+  using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
+  using WrappedResult = typename GoalHandle::WrappedResult;
+  using Feedback = typename ActionType::Feedback;
+
   // Constructor
   ClNav2Z(std::string actionServerName = "/navigate_to_pose") : actionServerName_(actionServerName)
   {
@@ -39,7 +45,7 @@ class ClNav2Z : public smacc2::ISmaccClient
   void onComponentInitialization()
   {
     // Create core action client component
-    auto actionClient = this->createComponent<
+    this->createComponent<
       smacc2::client_core_components::CpActionClient<nav2_msgs::action::NavigateToPose>,
       TOrthogonal, ClNav2Z>(actionServerName_);
 

smacc2_client_library/cl_nav2z/include/cl_nav2z/components/nav2_action_interface/cp_nav2_action_interface.hpp

@@ -124,25 +124,28 @@ class CpNav2ActionInterface : public smacc2::ISmaccComponent
     postNavigationSuccessEvent = [this](const WrappedResult & result)
     {
       auto * ev = new smacc2::default_events::EvActionSucceeded<TClient, TOrthogonal>();
+      ev->resultMessage = result;
       this->postEvent(ev);
     };
 
     postNavigationAbortedEvent = [this](const WrappedResult & result)
     {
       auto * ev = new smacc2::default_events::EvActionAborted<TClient, TOrthogonal>();
+      ev->resultMessage = result;
       this->postEvent(ev);
     };
 
     postNavigationCancelledEvent = [this](const WrappedResult & result)
     {
       auto * ev = new smacc2::default_events::EvActionCancelled<TClient, TOrthogonal>();
+      ev->resultMessage = result;
       this->postEvent(ev);
     };
 
     postNavigationFeedbackEvent = [this](const Feedback & feedback)
     {
-      auto * ev = new smacc2::default_events::EvActionFeedback<TClient, TOrthogonal>();
-      //ev->feedbackMessage = feedback;
+      auto * ev = new smacc2::default_events::EvActionFeedback<Feedback, TOrthogonal>();
+      ev->feedbackMessage = feedback;
       this->postEvent(ev);
     };
 

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/states/st_set_initial_pose.hpp

@@ -16,7 +16,7 @@
 
 #include <smacc2/smacc.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <cl_nav2z/client_behaviors/cb_wait_transform.hpp>
+#include <cl_nav2z/client_behaviors/cb_wait_nav2_nodes.hpp>
 
 namespace sm_nav2_unit_test_1
 {
@@ -25,32 +25,83 @@ using namespace cl_nav2z;
 using namespace cl_keyboard;
 using namespace smacc2::default_transition_tags;
 
-// Forward declaration
+// Custom transition tag for retry
+struct RETRY : SUCCESS {};
+
+// Custom event for max retries exceeded
+template <typename TSource, typename TOrthogonal>
+struct EvMaxRetriesExceeded : sc::event<EvMaxRetriesExceeded<TSource, TOrthogonal>>
+{
+};
+
+// Forward declarations
 struct StNavigateToWaypoint1;
+struct StFinalState;
 
 // STATE DECLARATION
 struct StSetInitialPose : smacc2::SmaccState<StSetInitialPose, SmNav2UnitTest1>
 {
   using SmaccState::SmaccState;
 
+  // Retry configuration: MAX_RETRIES means number of retries after initial attempt
+  // Total attempts = 1 (initial) + MAX_RETRIES = 4
+  static constexpr int MAX_RETRIES = 3;
+  static constexpr int MAX_ATTEMPTS = 1 + MAX_RETRIES;  // 4 total attempts
+  static constexpr const char* ATTEMPT_COUNTER_KEY = "localization_attempt_count";
+
   // TRANSITION TABLE
   typedef mpl::list<
-    Transition<EvCbSuccess<CbWaitTransform, OrNavigation>, StNavigateToWaypoint1, SUCCESS>,
-    Transition<EvCbFailure<CbWaitTransform, OrNavigation>, StNavigateToWaypoint1, ABORT>
+    // Success: proceed to navigation when Nav2 nodes are ready
+    Transition<EvCbSuccess<CbWaitNav2Nodes, OrNavigation>, StNavigateToWaypoint1, SUCCESS>,
+    // Failure with retries remaining: self-transition to try again
+    Transition<EvCbFailure<CbWaitNav2Nodes, OrNavigation>, StSetInitialPose, RETRY>,
+    // Max retries exceeded: abort to final state
+    Transition<EvMaxRetriesExceeded<CbWaitNav2Nodes, OrNavigation>, StFinalState, ABORT>
   > reactions;
 
   // STATE FUNCTIONS
   static void staticConfigure()
   {
-    // Wait for map->base_link transform (10 second timeout)
-    configure_orthogonal<OrNavigation, CbWaitTransform>("base_link", "map", rclcpp::Duration(10, 0));
+    // Wait for Nav2 nodes to be active (PlannerServer, ControllerServer, BtNavigator)
+    // This ensures the navigation stack is ready before attempting to navigate
+    configure_orthogonal<OrNavigation, CbWaitNav2Nodes>();
 
     // Keyboard behavior for manual control
     configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
   }
 
   void runtimeConfigure()
   {
+    // Increment attempt counter first
+    int attemptCount = 0;
+    this->getGlobalSMData(ATTEMPT_COUNTER_KEY, attemptCount);
+    attemptCount++;
+    this->setGlobalSMData(ATTEMPT_COUNTER_KEY, attemptCount);
+
+    // Check if max attempts exceeded (1 initial + MAX_RETRIES)
+    if (attemptCount > MAX_ATTEMPTS)
+    {
+      RCLCPP_ERROR(
+        getLogger(),
+        "StSetInitialPose: All %d attempts failed, aborting mission", MAX_ATTEMPTS);
+      // Post abort event - this will be processed and transition to StFinalState
+      this->postEvent<EvMaxRetriesExceeded<CbWaitNav2Nodes, OrNavigation>>();
+      return;
+    }
+
+    // Log current attempt
+    if (attemptCount == 1)
+    {
+      RCLCPP_INFO(getLogger(), "StSetInitialPose: Initial attempt (1 of %d)", MAX_ATTEMPTS);
+    }
+    else
+    {
+      RCLCPP_WARN(
+        getLogger(),
+        "StSetInitialPose: Retry %d of %d (attempt %d of %d)",
+        attemptCount - 1, MAX_RETRIES, attemptCount, MAX_ATTEMPTS);
+    }
+
     RCLCPP_INFO(getLogger(), "StSetInitialPose: runtimeConfigure() - Setting initial pose for AMCL");
 
     // Set initial pose for AMCL localization
@@ -81,12 +132,25 @@ struct StSetInitialPose : smacc2::SmaccState<StSetInitialPose, SmNav2UnitTest1>
 
   void onEntry()
   {
-    RCLCPP_INFO(getLogger(), "StSetInitialPose: onEntry() - Waiting for map->base_link transform");
+    RCLCPP_INFO(getLogger(), "StSetInitialPose: onEntry() - Waiting for Nav2 nodes to be active");
   }
 
   void onExit()
   {
-    RCLCPP_INFO(getLogger(), "StSetInitialPose: onExit() - Localization ready");
+    RCLCPP_INFO(getLogger(), "StSetInitialPose: onExit()");
+  }
+
+  // Reset attempt counter on successful exit to navigation
+  void onExit(SUCCESS)
+  {
+    // Clear attempt counter on success
+    this->setGlobalSMData(ATTEMPT_COUNTER_KEY, 0);
+    RCLCPP_INFO(getLogger(), "StSetInitialPose: Localization successful, proceeding to navigation");
+  }
+
+  void onExit(ABORT)
+  {
+    RCLCPP_ERROR(getLogger(), "StSetInitialPose: Localization failed after max retries, aborting");
   }
 };