#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 TSourceObject >
void	onComponentInitialization ()

void	onInitialize () override

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)

ActionClient *	getActionClient () const

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	onNavigationSuccess (const WrappedResult &result)

void	onNavigationAborted (const WrappedResult &result)

void	onNavigationCancelled (const WrappedResult &result)

void	onNavigationFeedback (const Feedback &feedback)

Goal	createNavigationGoal (const geometry_msgs::msg::PoseStamped &target)

Private Attributes
ActionClient *	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=false)

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

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 31 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 41 of file cp_nav2_action_interface.hpp.

◆ ActionType

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

Definition at line 35 of file cp_nav2_action_interface.hpp.

◆ Feedback

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

Definition at line 38 of file cp_nav2_action_interface.hpp.

◆ Goal

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

Definition at line 36 of file cp_nav2_action_interface.hpp.

◆ GoalHandle

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

Definition at line 39 of file cp_nav2_action_interface.hpp.

◆ Result

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

Definition at line 37 of file cp_nav2_action_interface.hpp.

◆ WrappedResult

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

Definition at line 40 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 89 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_, smacc2::client_core_components::CpActionClient< ActionType >::cancelGoal(), 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:

◆ createNavigationGoal()

Goal cl_nav2z::components::CpNav2ActionInterface::createNavigationGoal ( const geometry_msgs::msg::PoseStamped & target )

inlineprivate

Definition at line 246 of file cp_nav2_action_interface.hpp.

  {
    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
    }
 
    return goal;
  }

Referenced by sendNavigationGoal().

Here is the caller graph for this function:

◆ getActionClient()

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

inline

Definition at line 199 of file cp_nav2_action_interface.hpp.

199{ return actionClient_; }

References actionClient_.

◆ isNavigationServerReady()

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

inline

Definition at line 101 of file cp_nav2_action_interface.hpp.

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

smacc2::client_core_components::CpActionClient::isServerReady

bool isServerReady() const

Definition cp_action_client.hpp:139

References actionClient_, and smacc2::client_core_components::CpActionClient< ActionType >::isServerReady().

Here is the call graph for this function:

◆ onComponentInitialization()

template<typename TOrthogonal , typename TSourceObject >

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

inline

Definition at line 113 of file cp_nav2_action_interface.hpp.

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

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

Here is the call graph for this function:

◆ onInitialize()

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

inlineoverridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 147 of file cp_nav2_action_interface.hpp.

  {
    // 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
      actionClient_->onSucceeded(&CpNav2ActionInterface::onNavigationSuccess, this);
      actionClient_->onAborted(&CpNav2ActionInterface::onNavigationAborted, this);
      actionClient_->onCancelled(&CpNav2ActionInterface::onNavigationCancelled, this);
      actionClient_->onFeedback(&CpNav2ActionInterface::onNavigationFeedback, this);
 
      RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Signal connections established");
    }
    else
    {
      RCLCPP_ERROR(
        getLogger(), "[CpNav2ActionInterface] Action client not found during initialization!");
    }
  }

References actionClient_, smacc2::ISmaccComponent::getLogger(), smacc2::client_core_components::CpActionClient< ActionType >::onAborted(), smacc2::client_core_components::CpActionClient< ActionType >::onCancelled(), smacc2::client_core_components::CpActionClient< ActionType >::onFeedback(), onNavigationAborted(), onNavigationCancelled(), onNavigationFeedback(), onNavigationSuccess(), and smacc2::client_core_components::CpActionClient< ActionType >::onSucceeded().

Here is the call graph for this function:

◆ onNavigationAborted() [1/2]

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

inlineprivate

Definition at line 215 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.

Here is the call graph for this function:

◆ onNavigationAborted() [2/2]

template<typename T >

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

inline

Definition at line 177 of file cp_nav2_action_interface.hpp.

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

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

Referenced by onInitialize(), 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:

◆ onNavigationCancelled() [1/2]

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

inlineprivate

Definition at line 225 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.

Here is the call graph for this function:

◆ onNavigationCancelled() [2/2]

template<typename T >

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

inline

Definition at line 184 of file cp_nav2_action_interface.hpp.

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

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

Referenced by onInitialize(), 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:

◆ onNavigationFeedback() [1/2]

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

inlineprivate

Definition at line 235 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.

Here is the call graph for this function:

◆ onNavigationFeedback() [2/2]

template<typename T >

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

inline

Definition at line 192 of file cp_nav2_action_interface.hpp.

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

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

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 169 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:

◆ onNavigationSuccess()

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

inlineprivate

Definition at line 205 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 78 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_, smacc2::ISmaccComponent::getLogger(), and smacc2::client_core_components::CpActionClient< ActionType >::sendGoal().

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 59 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");
    }
 
    auto goal = createNavigationGoal(target);
 
    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_, createNavigationGoal(), smacc2::ISmaccComponent::getLogger(), and smacc2::client_core_components::CpActionClient< ActionType >::sendGoal().

Here is the call graph for this function:

◆ waitForNavigationServer()

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

inline

Definition at line 103 of file cp_nav2_action_interface.hpp.

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

References actionClient_, and smacc2::client_core_components::CpActionClient< ActionType >::waitForServer().

Here is the call graph for this function:

Member Data Documentation

◆ actionClient_

ActionClient* cl_nav2z::components::CpNav2ActionInterface::actionClient_ = nullptr

private

Definition at line 202 of file cp_nav2_action_interface.hpp.

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

◆ onNavigationAborted_

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

Definition at line 45 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationAborted().

◆ onNavigationCancelled_

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

Definition at line 46 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationCancelled().

◆ onNavigationFeedback_

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

Definition at line 47 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationFeedback().

◆ onNavigationSucceeded_

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

Definition at line 44 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationSuccess().

◆ postNavigationAbortedEvent

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

Definition at line 51 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationAborted().

◆ postNavigationCancelledEvent

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

Definition at line 52 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationCancelled().

◆ postNavigationFeedbackEvent

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

Definition at line 53 of file cp_nav2_action_interface.hpp.

Referenced by onComponentInitialization(), and onNavigationFeedback().

◆ postNavigationSuccessEvent

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

Definition at line 50 of file cp_nav2_action_interface.hpp.

Referenced by onNavigationSuccess().

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()

◆ createNavigationGoal()

◆ getActionClient()

◆ isNavigationServerReady()

◆ onComponentInitialization()

◆ onInitialize()

◆ onNavigationAborted() [1/2]

◆ onNavigationAborted() [2/2]

◆ onNavigationCancelled() [1/2]

◆ onNavigationCancelled() [2/2]

◆ onNavigationFeedback() [1/2]

◆ onNavigationFeedback() [2/2]

◆ onNavigationSucceeded()

◆ onNavigationSuccess()

◆ sendGoal()

◆ sendNavigationGoal()

◆ waitForNavigationServer()

Member Data Documentation

◆ actionClient_

◆ onNavigationAborted_

◆ onNavigationCancelled_

◆ onNavigationFeedback_

◆ onNavigationSucceeded_

◆ postNavigationAbortedEvent

◆ postNavigationCancelledEvent

◆ postNavigationFeedbackEvent

◆ postNavigationSuccessEvent