cb_action_client_behavior_base.hpp

Go to the documentation of this file.

// Copyright 2026 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
/*****************************************************************************************************************
 *
 *   Authors: Brett Aldrich, Pablo Inigo Blasco
 *
 ******************************************************************************************************************/
 
#pragma once
 
#include <atomic>
#include <chrono>
 
#include <smacc2/client_core_components/cp_action_client.hpp>
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
namespace smacc2
{
namespace client_behavior_bases
{
 
// Generic base for client behaviors that send one goal to a ROS 2 action server
// (through a CpActionClient<TAction> component) and finish when the action
// finishes. Derived behaviors fill a Goal in onEntry() and call sendGoal();
// the base turns the action result into the state-scoped behavior events
// EvCbSuccess<TDerived, TOrthogonal> / EvCbFailure<TDerived, TOrthogonal>.
//
// Lifecycle contract (see the async-thread locking rule in CLAUDE.md):
// - components are resolved and result signals wired in
//   onStateOrthogonalAllocation, on the state machine thread. Never from the
//   asynchronous onEntry/onExit threads.
// - a derived class that declares its own onStateOrthogonalAllocation MUST
//   chain to this one (and this one chains to SmaccAsyncClientBehavior's, which
//   installs the event-posting functions - skipping the chain leaves them empty
//   and postSuccessEvent() throws std::bad_function_call).
// - no machine-scoped events are posted by this base: transition tables react
//   to the EvCb* behavior events, which cannot leak across state transitions.
template <typename TAction>
class CbActionClientBehaviorBase : public smacc2::SmaccAsyncClientBehavior
{
public:
  using Goal = typename TAction::Goal;
  using GoalHandle = rclcpp_action::ClientGoalHandle<TAction>;
  using WrappedResult = typename GoalHandle::WrappedResult;
  using Feedback = typename TAction::Feedback;
  using ActionClientComponent = smacc2::client_core_components::CpActionClient<TAction>;
 
  template <typename TOrthogonal, typename TSourceObject>
  void onStateOrthogonalAllocation()
  {
    if (actionClientName_.empty())
    {
      this->requiresComponent(actionClient_, ComponentRequirement::HARD);
    }
    else
    {
      // target a specific named instance when the client holds several
      // CpActionClient components of the same action type
      this->requiresComponent(actionClientName_, actionClient_, ComponentRequirement::HARD);
    }
 
    if (!resultConnectionsInitialized_ && actionClient_ != nullptr)
    {
      actionClient_->onSucceeded(&CbActionClientBehaviorBase::dispatchActionSuccess, this);
      actionClient_->onAborted(&CbActionClientBehaviorBase::dispatchActionAbort, this);
      actionClient_->onCancelled(&CbActionClientBehaviorBase::dispatchActionAbort, this);
      actionClient_->onFeedback(&CbActionClientBehaviorBase::dispatchActionFeedback, this);
      resultConnectionsInitialized_ = true;
    }
 
    smacc2::SmaccAsyncClientBehavior::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
  }
 
  virtual ~CbActionClientBehaviorBase() {}
 
  // If the state exits while a goal is still in flight (e.g. a keyboard or
  // timeout transition), cancel it: unlike bt_navigator navigation - where the
  // next goal implicitly preempts - each behavior/docking server action keeps
  // executing an abandoned goal, leaving the robot moving under a command
  // nobody owns. Derived classes overriding onExit must chain to this.
  void onExit() override
  {
    if (goalInFlight_)
    {
      RCLCPP_WARN(
        getLogger(), "[%s] State exited with the action goal still in flight - cancelling",
        getName().c_str());
      cancelGoal();
    }
  }
 
protected:
  // Sends the goal and waits (in the calling asynchronous onEntry thread) for the
  // server's goal response. Returns true if the goal was accepted. On a
  // not-ready server, a rejected goal, or a response timeout it posts the
  // failure event and returns false - the caller can simply return from
  // onEntry(). This closes the silent-hang gap of a rejected/unanswered goal:
  // CpActionClient installs no goal_response handling, and a rejected goal
  // never invokes the result callback.
  bool sendGoal(Goal & goal)
  {
    if (actionClient_ == nullptr || !actionClient_->isServerReady())
    {
      RCLCPP_ERROR(
        getLogger(), "[%s] Action server not available, cannot send goal", getName().c_str());
      this->postFailureEvent();
      return false;
    }
 
    goalActivitySeen_ = false;
    auto goalHandleFuture = actionClient_->sendGoal(goal);
 
    // wait for goal acceptance in short slices so a state exit is honored
    auto deadline = std::chrono::steady_clock::now() + goalResponseTimeout_;
    while (!this->isShutdownRequested() && std::chrono::steady_clock::now() < deadline)
    {
      // Feedback or a result arriving proves the goal was accepted even if
      // the goal-response future hasn't been serviced yet: under spin_some, a
      // high-rate feedback stream (NavigateToPose feeds back continuously
      // from acceptance) can starve the action client's response processing
      // for the whole execution. Rejected goals produce no feedback, so their
      // response resolves the future promptly.
      if (goalActivitySeen_)
      {
        goalInFlight_ = true;
        return true;
      }
 
      if (goalHandleFuture.wait_for(std::chrono::milliseconds(50)) == std::future_status::ready)
      {
        if (goalHandleFuture.get() != nullptr)
        {
          goalInFlight_ = true;  // accepted; the result signals will finish the behavior
          return true;
        }
 
        RCLCPP_ERROR(getLogger(), "[%s] Goal was rejected by the action server", getName().c_str());
        this->postFailureEvent();
        return false;
      }
    }
 
    if (!this->isShutdownRequested())
    {
      // No response, no feedback, no result within the window: ambiguous, and
      // observed only under executor starvation with an accepted goal - a
      // genuine rejection resolves the future in milliseconds. Assume the
      // goal is running and let the result signals finish the behavior
      // (fire-and-forget, the pre-template behavior), rather than failing a
      // healthy mission.
      RCLCPP_WARN(
        getLogger(),
        "[%s] No goal response from the action server after %ld ms; assuming the goal was "
        "accepted (a rejection responds promptly) - result signals will finish this behavior",
        getName().c_str(), static_cast<long>(goalResponseTimeout_.count()));
      goalInFlight_ = true;
    }
    return true;
  }
 
  void cancelGoal()
  {
    if (actionClient_ != nullptr)
    {
      actionClient_->cancelGoal();
    }
  }
 
  // Result handlers connected by onStateOrthogonalAllocation. Base
  // implementations record the result code and post the behavior events;
  // derived classes may override to customize result handling.
  virtual void onActionSuccess(const WrappedResult & result)
  {
    actionResult_ = result.code;
    RCLCPP_INFO(getLogger(), "[%s] Action succeeded, propagating success event", getName().c_str());
    this->postSuccessEvent();
  }
 
  virtual void onActionAbort(const WrappedResult & result)
  {
    actionResult_ = result.code;
    RCLCPP_INFO(getLogger(), "[%s] Action failed, propagating failure event", getName().c_str());
    this->postFailureEvent();
  }
 
  // optional: override to consume action feedback (distance traveled etc.)
  virtual void onActionFeedback(const Feedback & /*feedback*/) {}
 
  // set from the derived constructor to bind to a named CpActionClient
  // instance; empty binds the first of matching type. Must be set before
  // onStateOrthogonalAllocation runs (i.e. NOT in runtimeConfigure, which
  // executes after allocation)
  void setActionClientName(std::string name) { actionClientName_ = std::move(name); }
 
  ActionClientComponent * actionClient_ = nullptr;
  std::string actionClientName_;
 
  rclcpp_action::ResultCode actionResult_ = rclcpp_action::ResultCode::UNKNOWN;
 
  // how long sendGoal waits for the server to accept/reject the goal
  std::chrono::milliseconds goalResponseTimeout_ = std::chrono::milliseconds(10000);
 
private:
  // trampolines wired to the component signals: clear the in-flight flag
  // regardless of what a derived handler override does, then dispatch to the
  // overridable virtuals
  void dispatchActionSuccess(const WrappedResult & result)
  {
    goalActivitySeen_ = true;
    goalInFlight_ = false;
    this->onActionSuccess(result);
  }
 
  void dispatchActionAbort(const WrappedResult & result)
  {
    goalActivitySeen_ = true;
    goalInFlight_ = false;
    this->onActionAbort(result);
  }
 
  void dispatchActionFeedback(const Feedback & feedback)
  {
    goalActivitySeen_ = true;
    this->onActionFeedback(feedback);
  }
 
  bool resultConnectionsInitialized_ = false;
  std::atomic<bool> goalInFlight_{false};
  std::atomic<bool> goalActivitySeen_{false};
};
 
}  // namespace client_behavior_bases
}  // namespace smacc2