#include <smacc_action_client_base.hpp>

Inheritance diagram for smacc2::client_bases::SmaccActionClientBase< ActionType >:

Collaboration diagram for smacc2::client_bases::SmaccActionClientBase< ActionType >:

Public Types
typedef rclcpp_action::Client< ActionType >	ActionClient

using	Goal = typename ActionClient::Goal

using	Feedback = typename ActionClient::Feedback

using	GoalHandle = rclcpp_action::ClientGoalHandle< ActionType >

typedef GoalHandle::WrappedResult	WrappedResult

using	SendGoalOptions = typename ActionClient::SendGoalOptions

using	GoalResponseCallback = std::function< void(std::shared_future< typename GoalHandle::SharedPtr >)>

using	FeedbackCallback = typename GoalHandle::FeedbackCallback

using	ResultCallback = typename GoalHandle::ResultCallback

using	CancelRequest = typename ActionType::Impl::CancelGoalService::Request

using	CancelResponse = typename ActionType::Impl::CancelGoalService::Response

using	CancelCallback = std::function< void(typename CancelResponse::SharedPtr)>

Public Member Functions
	SmaccActionClientBase (std::string actionServerName)

	SmaccActionClientBase ()

virtual	~SmaccActionClientBase ()

virtual std::shared_ptr< rclcpp_action::ClientBase >	getClientBase () override

void	onInitialize () override

template<typename EvType >
void	postResultEvent (WrappedResult &)

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

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

template<typename T >
boost::signals2::connection	onSucceeded (std::function< void(WrappedResult &)> callback)

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

template<typename T >
boost::signals2::connection	onAborted (std::function< void(WrappedResult &)> callback)

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

template<typename T >
boost::signals2::connection	onCancelled (std::function< void(WrappedResult &)> callback)

virtual bool	cancelGoal () override

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

Public Member Functions inherited from smacc2::client_bases::ISmaccActionClient
	ISmaccActionClient ()

virtual	~ISmaccActionClient ()

std::string	getNamespace () const

virtual bool	cancelGoal ()=0

virtual std::shared_ptr< rclcpp_action::ClientBase >	getClientBase ()=0

virtual std::string	getName () const

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

virtual	~ISmaccClient ()

virtual void	onInitialize ()

virtual std::string	getName () const

template<typename TComponent >
TComponent *	getComponent ()

template<typename TComponent >
TComponent *	getComponent (std::string name)

template<typename TComponent >
TComponent *	getComponent (int index)

virtual smacc2::introspection::TypeInfo::Ptr	getType ()

ISmaccStateMachine *	getStateMachine ()

template<typename TSmaccSignal , typename T >
void	connectSignal (TSmaccSignal &signal, void(T::callback)(), T object)

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

void	getComponents (std::vector< std::shared_ptr< ISmaccComponent > > &components)

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

template<typename EventType >
void	postEvent ()

Static Public Member Functions
static std::string	getEventLabel ()

Public Attributes
std::string	action_endpoint_

std::optional< std::shared_future< typename GoalHandle::SharedPtr > >	lastRequest_

GoalHandle::SharedPtr	goalHandle_

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

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

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

std::function< void(WrappedResult)>	postSuccessEvent

std::function< void(WrappedResult)>	postAbortedEvent

std::function< void(WrappedResult)>	postCancelledEvent

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

ResultCallback	done_cb

FeedbackCallback	feedback_cb

Protected Member Functions
void	onFeedback (typename GoalHandle::SharedPtr, const std::shared_ptr< const Feedback > feedback_msg)

void	onResult (const WrappedResult &result_msg)

Protected Member Functions inherited from smacc2::ISmaccClient
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

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

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

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger ()

Protected Attributes
ActionClient::SharedPtr	client_

Protected Attributes inherited from smacc2::client_bases::ISmaccActionClient
std::string	name_

Protected Attributes inherited from smacc2::ISmaccClient
std::map< ComponentKey, std::shared_ptr< smacc2::ISmaccComponent > >	components_

Detailed Description

template<typename ActionType>
class smacc2::client_bases::SmaccActionClientBase< ActionType >

Definition at line 37 of file smacc_action_client_base.hpp.

Member Typedef Documentation

◆ ActionClient

template<typename ActionType >

typedef rclcpp_action::Client<ActionType> smacc2::client_bases::SmaccActionClientBase< ActionType >::ActionClient

Definition at line 42 of file smacc_action_client_base.hpp.

◆ CancelCallback

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::CancelCallback = std::function<void(typename CancelResponse::SharedPtr)>

Definition at line 61 of file smacc_action_client_base.hpp.

◆ CancelRequest

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::CancelRequest = typename ActionType::Impl::CancelGoalService::Request

Definition at line 59 of file smacc_action_client_base.hpp.

◆ CancelResponse

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::CancelResponse = typename ActionType::Impl::CancelGoalService::Response

Definition at line 60 of file smacc_action_client_base.hpp.

◆ Feedback

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::Feedback = typename ActionClient::Feedback

Definition at line 50 of file smacc_action_client_base.hpp.

◆ FeedbackCallback

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::FeedbackCallback = typename GoalHandle::FeedbackCallback

Definition at line 57 of file smacc_action_client_base.hpp.

◆ Goal

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::Goal = typename ActionClient::Goal

Definition at line 49 of file smacc_action_client_base.hpp.

◆ GoalHandle

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>

Definition at line 51 of file smacc_action_client_base.hpp.

◆ GoalResponseCallback

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::GoalResponseCallback = std::function<void(std::shared_future<typename GoalHandle::SharedPtr>)>

Definition at line 55 of file smacc_action_client_base.hpp.

◆ ResultCallback

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::ResultCallback = typename GoalHandle::ResultCallback

Definition at line 58 of file smacc_action_client_base.hpp.

◆ SendGoalOptions

template<typename ActionType >

using smacc2::client_bases::SmaccActionClientBase< ActionType >::SendGoalOptions = typename ActionClient::SendGoalOptions

Definition at line 54 of file smacc_action_client_base.hpp.

◆ WrappedResult

template<typename ActionType >

typedef GoalHandle::WrappedResult smacc2::client_bases::SmaccActionClientBase< ActionType >::WrappedResult

Definition at line 52 of file smacc_action_client_base.hpp.

Constructor & Destructor Documentation

◆ SmaccActionClientBase() [1/2]

template<typename ActionType >

smacc2::client_bases::SmaccActionClientBase< ActionType >::SmaccActionClientBase ( std::string actionServerName )

inline

Definition at line 64 of file smacc_action_client_base.hpp.

                                                    : ISmaccActionClient()
  {
    action_endpoint_ = actionServerName;
  }

References smacc2::client_bases::SmaccActionClientBase< ActionType >::action_endpoint_.

◆ SmaccActionClientBase() [2/2]

template<typename ActionType >

smacc2::client_bases::SmaccActionClientBase< ActionType >::SmaccActionClientBase ( )

inline

Definition at line 69 of file smacc_action_client_base.hpp.

69: ISmaccActionClient() { name_ = ""; }

smacc2::client_bases::ISmaccActionClient::name_

std::string name_

Definition: smacc_action_client.hpp:54

References smacc2::client_bases::ISmaccActionClient::name_.

◆ ~SmaccActionClientBase()

template<typename ActionType >

virtual smacc2::client_bases::SmaccActionClientBase< ActionType >::~SmaccActionClientBase ( )

inlinevirtual

Definition at line 71 of file smacc_action_client_base.hpp.

71{}

Member Function Documentation

◆ cancelGoal()

template<typename ActionType >

virtual bool smacc2::client_bases::SmaccActionClientBase< ActionType >::cancelGoal ( )

inlineoverridevirtual

Implements smacc2::client_bases::ISmaccActionClient.

Definition at line 214 of file smacc_action_client_base.hpp.

  {
    if (lastRequest_ && lastRequest_->valid())
    {
      rclcpp::spin_until_future_complete(getNode(), *lastRequest_);
      auto req = lastRequest_->get();
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName() << "] Cancelling goal. req id: "
                         << rclcpp_action::to_string(req->get_goal_id()));
      auto cancelresult = client_->async_cancel_goal(req);
 
      // wait actively
      rclcpp::spin_until_future_complete(getNode(), cancelresult);
      //lastRequest_.reset();
      return true;
    }
    else
    {
      RCLCPP_ERROR(
        getLogger(), "%s [at %s]: not connected with actionserver, skipping cancel goal ...",
        getName().c_str(), getNamespace().c_str());
      return false;
    }
  }

References smacc2::client_bases::SmaccActionClientBase< ActionType >::client_, smacc2::ISmaccClient::getLogger(), smacc2::client_bases::ISmaccActionClient::getName(), smacc2::client_bases::ISmaccActionClient::getNamespace(), smacc2::ISmaccClient::getNode(), and smacc2::client_bases::SmaccActionClientBase< ActionType >::lastRequest_.

Here is the call graph for this function:

◆ getClientBase()

template<typename ActionType >

virtual std::shared_ptr< rclcpp_action::ClientBase > smacc2::client_bases::SmaccActionClientBase< ActionType >::getClientBase ( )

inlineoverridevirtual

Implements smacc2::client_bases::ISmaccActionClient.

Definition at line 73 of file smacc_action_client_base.hpp.

73{ return client_; }

References smacc2::client_bases::SmaccActionClientBase< ActionType >::client_.

◆ getEventLabel()

template<typename ActionType >

static std::string smacc2::client_bases::SmaccActionClientBase< ActionType >::getEventLabel ( )

inlinestatic

Definition at line 85 of file smacc_action_client_base.hpp.

  {
    auto type = TypeInfo::getTypeInfoFromType<ActionType>();
    return type->getNonTemplatedTypeName();
  }

References generate_debs::type.

◆ onAborted() [1/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onAborted ( std::function< void(WrappedResult &)> callback )

inline

Definition at line 171 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Here is the call graph for this function:

◆ onAborted() [2/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onAborted	(	void(T::*)(WrappedResult &)	callback,
		T *	object
	)

inline

Definition at line 165 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onOrthogonalAllocation(), and cl_nav2z::WaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onCancelled() [1/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onCancelled ( std::function< void(WrappedResult &)> callback )

inline

Definition at line 183 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Here is the call graph for this function:

◆ onCancelled() [2/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onCancelled	(	void(T::*)(WrappedResult &)	callback,
		T *	object
	)

inline

Definition at line 177 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onOrthogonalAllocation(), and cl_nav2z::WaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onFeedback()

template<typename ActionType >

void smacc2::client_bases::SmaccActionClientBase< ActionType >::onFeedback	(	typename GoalHandle::SharedPtr	,
		const std::shared_ptr< const Feedback >	feedback_msg
	)

inlineprotected

Definition at line 342 of file smacc_action_client_base.hpp.

  {
    postFeedbackEvent(*feedback_msg);
  }

References smacc2::client_bases::SmaccActionClientBase< ActionType >::postFeedbackEvent.

Referenced by smacc2::client_bases::SmaccActionClientBase< ActionType >::onOrthogonalAllocation().

Here is the caller graph for this function:

◆ onInitialize()

template<typename ActionType >

void smacc2::client_bases::SmaccActionClientBase< ActionType >::onInitialize ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClient.

Definition at line 75 of file smacc_action_client_base.hpp.

  {
    if (name_ == "") name_ = smacc2::demangleSymbol(typeid(*this).name());
    this->client_ = rclcpp_action::create_client<ActionType>(getNode(), action_endpoint_);
    // RCLCPP_INFO_STREAM(
    //   this->getLogger(),
    //   "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
    //client_->wait_for_action_server();
  }

References smacc2::client_bases::SmaccActionClientBase< ActionType >::action_endpoint_, smacc2::client_bases::SmaccActionClientBase< ActionType >::client_, smacc2::introspection::demangleSymbol(), smacc2::ISmaccClient::getNode(), and smacc2::client_bases::ISmaccActionClient::name_.

Here is the call graph for this function:

◆ onOrthogonalAllocation()

template<typename ActionType >

template<typename TOrthogonal , typename TSourceObject >

void smacc2::client_bases::SmaccActionClientBase< ActionType >::onOrthogonalAllocation ( )

inline

Definition at line 125 of file smacc_action_client_base.hpp.

  {
    // we create here all the event factory functions capturing the TOrthogonal
    postSuccessEvent = [this](auto msg) {
      this->postResultEvent<EvActionSucceeded<TSourceObject, TOrthogonal>>(msg);
    };
    postAbortedEvent = [this](auto msg) {
      this->postResultEvent<EvActionAborted<TSourceObject, TOrthogonal>>(msg);
    };
 
    postCancelledEvent = [this](auto msg) {
      this->postResultEvent<EvActionCancelled<TSourceObject, TOrthogonal>>(msg);
    };
    postFeedbackEvent = [this](auto msg) {
      auto actionFeedbackEvent = new EvActionFeedback<Feedback, TOrthogonal>();
      actionFeedbackEvent->client = this;
      actionFeedbackEvent->feedbackMessage = msg;
      this->postEvent(actionFeedbackEvent);
      RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
    };
 
    done_cb = [this](auto r) { this->onResult(r); };
    // done_cb = boost::bind(&SmaccActionClientBase<ActionType>::onResult, this, _1, _2);
    // active_cb;
    feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
  }

Here is the call graph for this function:

◆ onResult()

template<typename ActionType >

void smacc2::client_bases::SmaccActionClientBase< ActionType >::onResult ( const WrappedResult & result_msg )

inlineprotected

Definition at line 349 of file smacc_action_client_base.hpp.

  {
    // auto *actionResultEvent = new EvActionResult<TDerived>();
    // actionResultEvent->client = this;
    // actionResultEvent->resultMessage = *result_msg;
 
    // const auto &resultType = this->getState();
    const auto & resultType = result_msg.code;
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << this->getName() << "] request result of request ["
                       << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
 
    if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
    {
      RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
      onSucceeded_(result_msg);
      postSuccessEvent(result_msg);
    }
    else if (resultType == rclcpp_action::ResultCode::ABORTED)
    {
      RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
      onAborted_(result_msg);
      postAbortedEvent(result_msg);
    }
    else if (resultType == rclcpp_action::ResultCode::CANCELED)
    {
      RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
      onCancelled_(result_msg);
      postCancelledEvent(result_msg);
    }
    /*
    else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
    {
        RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
        onRejected_(result_msg);
        postRejectedEvent(result_msg);
    }
    else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
    {
        RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
        onPreempted_(result_msg);
        postPreemptedEvent(result_msg);
    }*/
    else
    {
      RCLCPP_INFO(
        getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
        (int)resultType);
    }
  }

References smacc2::ISmaccClient::getLogger(), smacc2::client_bases::ISmaccActionClient::getName(), smacc2::client_bases::SmaccActionClientBase< ActionType >::onAborted_, smacc2::client_bases::SmaccActionClientBase< ActionType >::onCancelled_, smacc2::client_bases::SmaccActionClientBase< ActionType >::onSucceeded_, smacc2::client_bases::SmaccActionClientBase< ActionType >::postAbortedEvent, smacc2::client_bases::SmaccActionClientBase< ActionType >::postCancelledEvent, and smacc2::client_bases::SmaccActionClientBase< ActionType >::postSuccessEvent.

Referenced by smacc2::client_bases::SmaccActionClientBase< ActionType >::onOrthogonalAllocation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onSucceeded() [1/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onSucceeded ( std::function< void(WrappedResult &)> callback )

inline

Definition at line 159 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Here is the call graph for this function:

◆ onSucceeded() [2/2]

template<typename ActionType >

template<typename T >

boost::signals2::connection smacc2::client_bases::SmaccActionClientBase< ActionType >::onSucceeded	(	void(T::*)(WrappedResult &)	callback,
		T *	object
	)

inline

Definition at line 153 of file smacc_action_client_base.hpp.

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

References callback(), smacc2::ISmaccStateMachine::createSignalConnection(), and smacc2::ISmaccClient::getStateMachine().

Referenced by cl_nav2z::CbNav2ZClientBehaviorBase::onOrthogonalAllocation(), sm_dance_bot::StNavigateForward1::runtimeConfigure(), sm_dance_bot_strikes_back::StNavigateForward1::runtimeConfigure(), and cl_nav2z::WaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ postResultEvent()

template<typename ActionType >

template<typename EvType >

void smacc2::client_bases::SmaccActionClientBase< ActionType >::postResultEvent ( WrappedResult & )

inline

Definition at line 114 of file smacc_action_client_base.hpp.

  {
    auto * ev = new EvType();
    // ev->client = this;
    // ev->resultMessage = *result;
    RCLCPP_INFO(
      getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
    this->postEvent(ev);
  }

References smacc2::introspection::demangleSymbol(), smacc2::ISmaccClient::getLogger(), and smacc2::ISmaccClient::postEvent().

Here is the call graph for this function:

◆ sendGoal()

template<typename ActionType >

std::shared_future< typename GoalHandle::SharedPtr > smacc2::client_bases::SmaccActionClientBase< ActionType >::sendGoal ( Goal & goal )

inline

Function called whenever feedback is received for the goal.

Function called when the result for the goal is received.

Definition at line 239 of file smacc_action_client_base.hpp.

  {
    // client_->sendGoal(goal, done_cb, active_cb, feedback_cb);
    // std::shared_future<typename GoalHandle::SharedPtr>
 
    SendGoalOptions options;
 
    // GoalResponseCallback
    // options.goal_response_callback;
 
    // FeedbackCallback
    options.feedback_callback = feedback_cb;
 
    // ResultCallback result_callback;
    // options.result_callback = done_cb;
 
    options.result_callback =
      [this](const typename rclcpp_action::ClientGoalHandle<ActionType>::WrappedResult & result) {
        // TODO(#1652): a work around until rcl_action interface is updated
        // if goal ids are not matched, the older goal call this callback so ignore the result
        // if matched, it must be processed (including aborted)
        RCLCPP_INFO_STREAM(getLogger(), getName() << ": Result callback, getting shared future");
        goalHandle_ = lastRequest_->get();
        RCLCPP_INFO_STREAM(getLogger(), getName() << ": Result CB Check goal id");
        if (this->goalHandle_->get_goal_id() == result.goal_id)
        {
          // goal_result_available_ = true;
          // result_ = result;
          RCLCPP_INFO_STREAM(getLogger(), getName() << ": Result CB Goal id matches");
          done_cb(result);
        }
        else
        {
          RCLCPP_INFO_STREAM(getLogger(), getName() << ": Result CB Goal id DOES NOT match");
        }
      };
 
    // if (lastRequest_ && lastRequest_->valid())
    // {
    //   RCLCPP_INFO_STREAM(getLogger(), getName() << ": checking previous request is really finished.");
    //   auto res = this->lastRequest_->get();
    //   RCLCPP_INFO_STREAM(getLogger(), getName() << ": okay");
    // }
    // else
    // {
    //   RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
    // }
 
    RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
    this->lastRequest_ = this->client_->async_send_goal(goal, options);
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] Action goal sent to " << this->action_endpoint_
                       << "\": " << std::endl
                       << goal);
 
    // if (client_->isServerConnected())
    // {
    // RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal sent:" << goal);
 
    // RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal Id: "  <<
    // rclcpp_action::to_string(lastRequest_->get()->get_goal_id()));
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] client ready clients: "
                       << this->client_->get_number_of_ready_clients());
    RCLCPP_INFO_STREAM(
      getLogger(),
      "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
 
    // for (auto& gh: this->goal_handles_)
    // {
 
    // }
 
    // RCLCPP_INFO_STREAM(getLogger(), getName() << ": spinning until completed");
    // if (rclcpp::spin_until_future_complete(this->getNode(), lastRequest_, std::chrono::seconds(2))
    // !=rclcpp::executor::FutureReturnCode::SUCCESS)
    // {
    //   throw std::runtime_error("send_goal failed");
    // }
 
    // goalHandle_ = lastRequest_->get();
    // if (!goalHandle_) {
    //   throw std::runtime_error("Goal was rejected by the action server");
    // }
 
    // }
    // else
    // {
    //     RCLCPP_ERROR(getLogger(),"%s [at %s]: not connected with actionserver, skipping goal request
    //     ...", getName().c_str(), getNamespace().c_str());
    //     //client_->waitForServer();
    // }
 
    return *lastRequest_;
  }