#include <cp_nav2_action_interface.hpp>

Inheritance diagram for cl_nav2z::components::CpNav2ActionInterface:

Collaboration diagram for cl_nav2z::components::CpNav2ActionInterface:

Public Types
using	ActionType = nav2_msgs::action::NavigateToPose

using	Goal = ActionType::Goal

using	Result = ActionType::Result

using	Feedback = ActionType::Feedback

using	GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>

using	WrappedResult = typename GoalHandle::WrappedResult

using	ActionClient = smacc2::client_core_components::CpActionClient<ActionType>

Public Member Functions
	CpNav2ActionInterface ()=default

virtual	~CpNav2ActionInterface ()=default

std::shared_future< typename GoalHandle::SharedPtr >	sendNavigationGoal (const geometry_msgs::msg::PoseStamped &target)

std::shared_future< typename GoalHandle::SharedPtr >	sendGoal (Goal &goal)

bool	cancelNavigation ()

bool	isNavigationServerReady () const

void	waitForNavigationServer ()

template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

void	onInitialize () override

ActionClient *	getActionClient () const

template<typename T >
boost::signals2::connection	onNavigationSucceeded (void(T::callback)(const WrappedResult &), T object)

template<typename T >
boost::signals2::connection	onNavigationAborted (void(T::callback)(const WrappedResult &), T object)

template<typename T >
boost::signals2::connection	onNavigationCancelled (void(T::callback)(const WrappedResult &), T object)

template<typename T >
boost::signals2::connection	onNavigationFeedback (void(T::callback)(const Feedback &), T object)

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Attributes
smacc2::SmaccSignal< void(const WrappedResult &)>	onNavigationSucceeded_

smacc2::SmaccSignal< void(const WrappedResult &)>	onNavigationAborted_

smacc2::SmaccSignal< void(const WrappedResult &)>	onNavigationCancelled_

smacc2::SmaccSignal< void(const Feedback &)>	onNavigationFeedback_

std::function< void(const WrappedResult &)>	postNavigationSuccessEvent

std::function< void(const WrappedResult &)>	postNavigationAbortedEvent

std::function< void(const WrappedResult &)>	postNavigationCancelledEvent

std::function< void(const Feedback &)>	postNavigationFeedbackEvent

Private Member Functions
void	onNavigationSuccessCallback (const WrappedResult &result)

void	onNavigationAbortedCallback (const WrappedResult &result)

void	onNavigationCancelledCallback (const WrappedResult &result)

void	onNavigationFeedbackCallback (const Feedback &feedback)

Private Attributes
smacc2::client_core_components::CpActionClient< ActionType > *	actionClient_ = nullptr

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Definition at line 32 of file cp_nav2_action_interface.hpp.

Member Typedef Documentation

◆ ActionClient

using cl_nav2z::components::CpNav2ActionInterface::ActionClient = smacc2::client_core_components::CpActionClient<ActionType>

Definition at line 42 of file cp_nav2_action_interface.hpp.

◆ ActionType

using cl_nav2z::components::CpNav2ActionInterface::ActionType = nav2_msgs::action::NavigateToPose

Definition at line 36 of file cp_nav2_action_interface.hpp.

◆ Feedback

using cl_nav2z::components::CpNav2ActionInterface::Feedback = ActionType::Feedback

Definition at line 39 of file cp_nav2_action_interface.hpp.

◆ Goal

using cl_nav2z::components::CpNav2ActionInterface::Goal = ActionType::Goal

Definition at line 37 of file cp_nav2_action_interface.hpp.

◆ GoalHandle

using cl_nav2z::components::CpNav2ActionInterface::GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>

Definition at line 40 of file cp_nav2_action_interface.hpp.

◆ Result

using cl_nav2z::components::CpNav2ActionInterface::Result = ActionType::Result

Definition at line 38 of file cp_nav2_action_interface.hpp.

◆ WrappedResult

using cl_nav2z::components::CpNav2ActionInterface::WrappedResult = typename GoalHandle::WrappedResult

Definition at line 41 of file cp_nav2_action_interface.hpp.

Constructor & Destructor Documentation

◆ CpNav2ActionInterface()

cl_nav2z::components::CpNav2ActionInterface::CpNav2ActionInterface ( )

default

◆ ~CpNav2ActionInterface()

virtual cl_nav2z::components::CpNav2ActionInterface::~CpNav2ActionInterface ( )

virtualdefault

Member Function Documentation

◆ cancelNavigation()

bool cl_nav2z::components::CpNav2ActionInterface::cancelNavigation ( )

inline

Definition at line 97 of file cp_nav2_action_interface.hpp.

  {
    if (!actionClient_)
    {
      RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Action client not available for cancel!");
      return false;
    }
 
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Cancelling navigation goal");
    return actionClient_->cancelGoal();
  }

References actionClient_, and smacc2::ISmaccComponent::getLogger().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::cancelGoal(), and cl_nav2z::CbAbortNavigation::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getActionClient()

ActionClient * cl_nav2z::components::CpNav2ActionInterface::getActionClient ( ) const

inline

Definition at line 180 of file cp_nav2_action_interface.hpp.

180{ return actionClient_; }

References actionClient_.

◆ isNavigationServerReady()

bool cl_nav2z::components::CpNav2ActionInterface::isNavigationServerReady ( ) const

inline

Definition at line 109 of file cp_nav2_action_interface.hpp.

109{ return actionClient_ && actionClient_->isServerReady(); }

References actionClient_.

◆ onComponentInitialization()

template<typename TOrthogonal , typename TClient >

void cl_nav2z::components::CpNav2ActionInterface::onComponentInitialization ( )

inline

Definition at line 121 of file cp_nav2_action_interface.hpp.

  {
    // Set up nav2-specific event posting functions
    postNavigationSuccessEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionSucceeded<TClient, TOrthogonal>();
      this->postEvent(ev);
    };
 
    postNavigationAbortedEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionAborted<TClient, TOrthogonal>();
      this->postEvent(ev);
    };
 
    postNavigationCancelledEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionCancelled<TClient, TOrthogonal>();
      this->postEvent(ev);
    };
 
    postNavigationFeedbackEvent = [this](const Feedback & feedback)
    {
      auto * ev = new smacc2::default_events::EvActionFeedback<TClient, TOrthogonal>();
      //ev->feedbackMessage = feedback;
      this->postEvent(ev);
    };
 
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Component initialized");
  }

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::postEvent(), postNavigationAbortedEvent, postNavigationCancelledEvent, postNavigationFeedbackEvent, and postNavigationSuccessEvent.

Here is the call graph for this function:

◆ onInitialize()

void cl_nav2z::components::CpNav2ActionInterface::onInitialize ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 152 of file cp_nav2_action_interface.hpp.

  {
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Component initializing");
 
    // Require the underlying action client component
    this->requiresComponent(actionClient_, ComponentRequirement::HARD);
 
    // Wire up signal connections from action client to nav2-specific signals
    if (actionClient_)
    {
      // Connect action client signals to our nav2-specific signals and event posting using method pointers
      // these are the main smacc signals in the action client, our navigation client behavior subscribes to them and reacts accordingly
      // each derived behavior do something different in reaction to these signals
      actionClient_->onSucceeded(&CpNav2ActionInterface::onNavigationSuccessCallback, this);
      actionClient_->onAborted(&CpNav2ActionInterface::onNavigationAbortedCallback, this);
      actionClient_->onCancelled(&CpNav2ActionInterface::onNavigationCancelledCallback, this);
      actionClient_->onFeedback(&CpNav2ActionInterface::onNavigationFeedbackCallback, this);
 
      RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Signal connections established");
    }
    else
    {
      RCLCPP_ERROR(
        getLogger(), "[CpNav2ActionInterface] Action client not found during initialization!");
    }
  }

References actionClient_, smacc2::ISmaccComponent::getLogger(), onNavigationAbortedCallback(), onNavigationCancelledCallback(), onNavigationFeedbackCallback(), onNavigationSuccessCallback(), and smacc2::ISmaccComponent::requiresComponent().

Here is the call graph for this function:

◆ onNavigationAborted()

template<typename T >

boost::signals2::connection cl_nav2z::components::CpNav2ActionInterface::onNavigationAborted	(	void(T::*	callback )(const WrappedResult &),
		T *	object )

inline

Definition at line 192 of file cp_nav2_action_interface.hpp.

  {
    return this->getStateMachine()->createSignalConnection(onNavigationAborted_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccComponent::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onNavigationAborted(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationAbortedCallback()

void cl_nav2z::components::CpNav2ActionInterface::onNavigationAbortedCallback ( const WrappedResult & result )

inlineprivate

Definition at line 227 of file cp_nav2_action_interface.hpp.

  {
    RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Navigation aborted");
    onNavigationAborted_(result);
    if (postNavigationAbortedEvent)
    {
      postNavigationAbortedEvent(result);
    }
  }

References smacc2::ISmaccComponent::getLogger(), onNavigationAborted_, and postNavigationAbortedEvent.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationCancelled()

template<typename T >

boost::signals2::connection cl_nav2z::components::CpNav2ActionInterface::onNavigationCancelled	(	void(T::*	callback )(const WrappedResult &),
		T *	object )

inline

Definition at line 199 of file cp_nav2_action_interface.hpp.

  {
    return this->getStateMachine()->createSignalConnection(
      onNavigationCancelled_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccComponent::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onNavigationCancelled(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationCancelledCallback()

void cl_nav2z::components::CpNav2ActionInterface::onNavigationCancelledCallback ( const WrappedResult & result )

inlineprivate

Definition at line 237 of file cp_nav2_action_interface.hpp.

  {
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation cancelled");
    onNavigationCancelled_(result);
    if (postNavigationCancelledEvent)
    {
      postNavigationCancelledEvent(result);
    }
  }

References smacc2::ISmaccComponent::getLogger(), onNavigationCancelled_, and postNavigationCancelledEvent.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationFeedback()

template<typename T >

boost::signals2::connection cl_nav2z::components::CpNav2ActionInterface::onNavigationFeedback	(	void(T::*	callback )(const Feedback &),
		T *	object )

inline

Definition at line 207 of file cp_nav2_action_interface.hpp.

  {
    return this->getStateMachine()->createSignalConnection(onNavigationFeedback_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccComponent::getStateMachine().

Here is the call graph for this function:

◆ onNavigationFeedbackCallback()

void cl_nav2z::components::CpNav2ActionInterface::onNavigationFeedbackCallback ( const Feedback & feedback )

inlineprivate

Definition at line 247 of file cp_nav2_action_interface.hpp.

  {
    RCLCPP_DEBUG(getLogger(), "[CpNav2ActionInterface] Navigation feedback received");
    onNavigationFeedback_(feedback);
    if (postNavigationFeedbackEvent)
    {
      postNavigationFeedbackEvent(feedback);
    }
  }

References smacc2::ISmaccComponent::getLogger(), onNavigationFeedback_, and postNavigationFeedbackEvent.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationSucceeded()

template<typename T >

boost::signals2::connection cl_nav2z::components::CpNav2ActionInterface::onNavigationSucceeded	(	void(T::*	callback )(const WrappedResult &),
		T *	object )

inline

Definition at line 184 of file cp_nav2_action_interface.hpp.

  {
    return this->getStateMachine()->createSignalConnection(
      onNavigationSucceeded_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccComponent::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onNavigationSucceeded(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNavigationSuccessCallback()

void cl_nav2z::components::CpNav2ActionInterface::onNavigationSuccessCallback ( const WrappedResult & result )

inlineprivate

Definition at line 217 of file cp_nav2_action_interface.hpp.

  {
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation succeeded");
    onNavigationSucceeded_(result);
    if (postNavigationSuccessEvent)
    {
      postNavigationSuccessEvent(result);
    }
  }

References smacc2::ISmaccComponent::getLogger(), onNavigationSucceeded_, and postNavigationSuccessEvent.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sendGoal()

std::shared_future< typename GoalHandle::SharedPtr > cl_nav2z::components::CpNav2ActionInterface::sendGoal ( Goal & goal )

inline

Definition at line 86 of file cp_nav2_action_interface.hpp.

  {
    if (!actionClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
      throw std::runtime_error("Action client not initialized");
    }
 
    return actionClient_->sendGoal(goal);
  }

References actionClient_, and smacc2::ISmaccComponent::getLogger().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), and cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sendNavigationGoal()

std::shared_future< typename GoalHandle::SharedPtr > cl_nav2z::components::CpNav2ActionInterface::sendNavigationGoal ( const geometry_msgs::msg::PoseStamped & target )

inline

Definition at line 60 of file cp_nav2_action_interface.hpp.

  {
    if (!actionClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
      throw std::runtime_error("Action client not initialized");
    }
 
    Goal goal;
    goal.pose = target;
 
    // Set default behavior_tree if not specified
    if (goal.behavior_tree.empty())
    {
      goal.behavior_tree = "";  // Let Nav2 use default behavior tree
    }
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[CpNav2ActionInterface] Sending navigation goal to: "
                     << "x=" << target.pose.position.x << ", y=" << target.pose.position.y
                     << ", frame=" << target.header.frame_id);
 
    return actionClient_->sendGoal(goal);
  }

References actionClient_, and smacc2::ISmaccComponent::getLogger().

Here is the call graph for this function:

◆ waitForNavigationServer()

void cl_nav2z::components::CpNav2ActionInterface::waitForNavigationServer ( )

inline

Definition at line 111 of file cp_nav2_action_interface.hpp.

  {
    if (actionClient_)
    {
      actionClient_->waitForServer();
    }
  }

References actionClient_.

Member Data Documentation

◆ actionClient_

smacc2::client_core_components::CpActionClient<ActionType>* cl_nav2z::components::CpNav2ActionInterface::actionClient_ = nullptr

private

Definition at line 214 of file cp_nav2_action_interface.hpp.

Referenced by cancelNavigation(), getActionClient(), isNavigationServerReady(), onInitialize(), sendGoal(), sendNavigationGoal(), and waitForNavigationServer().

◆ onNavigationAborted_

smacc2::SmaccSignal<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::onNavigationAborted_

Definition at line 46 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationAbortedCallback().

◆ onNavigationCancelled_

smacc2::SmaccSignal<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::onNavigationCancelled_

Definition at line 47 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationCancelledCallback().

◆ onNavigationFeedback_

smacc2::SmaccSignal<void(const Feedback &)> cl_nav2z::components::CpNav2ActionInterface::onNavigationFeedback_

Definition at line 48 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationFeedbackCallback().

◆ onNavigationSucceeded_

smacc2::SmaccSignal<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::onNavigationSucceeded_

Definition at line 45 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationSuccessCallback().

◆ postNavigationAbortedEvent

std::function<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::postNavigationAbortedEvent

Definition at line 52 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationAbortedCallback().

◆ postNavigationCancelledEvent

std::function<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::postNavigationCancelledEvent

Definition at line 53 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationCancelledCallback().

◆ postNavigationFeedbackEvent

std::function<void(const Feedback &)> cl_nav2z::components::CpNav2ActionInterface::postNavigationFeedbackEvent

Definition at line 54 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationFeedbackCallback().

◆ postNavigationSuccessEvent

std::function<void(const WrappedResult &)> cl_nav2z::components::CpNav2ActionInterface::postNavigationSuccessEvent

Definition at line 51 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationSuccessCallback().

The documentation for this class was generated from the following file:

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

Public Types

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ ActionClient

◆ ActionType

◆ Feedback

◆ Goal

◆ GoalHandle

◆ Result

◆ WrappedResult

Constructor & Destructor Documentation

◆ CpNav2ActionInterface()

◆ ~CpNav2ActionInterface()

Member Function Documentation

◆ cancelNavigation()

◆ getActionClient()

◆ isNavigationServerReady()

◆ onComponentInitialization()

◆ onInitialize()

◆ onNavigationAborted()

◆ onNavigationAbortedCallback()

◆ onNavigationCancelled()

◆ onNavigationCancelledCallback()

◆ onNavigationFeedback()

◆ onNavigationFeedbackCallback()

◆ onNavigationSucceeded()

◆ onNavigationSuccessCallback()

◆ sendGoal()

◆ sendNavigationGoal()

◆ waitForNavigationServer()

Member Data Documentation

◆ actionClient_

◆ onNavigationAborted_

◆ onNavigationCancelled_

◆ onNavigationFeedback_

◆ onNavigationSucceeded_

◆ postNavigationAbortedEvent

◆ postNavigationCancelledEvent

◆ postNavigationFeedbackEvent

◆ postNavigationSuccessEvent