cl_nav2z::CpWaypointNavigator Class Reference

#include <cp_waypoints_navigator.hpp>

Inheritance diagram for cl_nav2z::CpWaypointNavigator:

Collaboration diagram for cl_nav2z::CpWaypointNavigator:

Public Member Functions
	CpWaypointNavigator ()
void	onInitialize () override
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > >	sendNextGoal (std::optional< NavigateNextWaypointOptions > options=std::nullopt)
void	stopWaitingResult ()
	CpWaypointNavigator ()
void	onInitialize () override
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > >	sendNextGoal (std::optional< NavigateNextWaypointOptions > options=std::nullopt, cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr callback=cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr())
void	stopWaitingResult ()
Public Member Functions inherited from cl_nav2z::CpWaypointNavigatorBase
	CpWaypointNavigatorBase ()
virtual	~CpWaypointNavigatorBase ()
void	onInitialize () override
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
void	loadWayPointsFromFile (std::string filepath)
void	loadWayPointsFromFile2 (std::string filepath)
void	setWaypoints (const std::vector< geometry_msgs::msg::Pose > &waypoints)
void	setWaypoints (const std::vector< Pose2D > &waypoints)
const std::vector< geometry_msgs::msg::Pose > &	getWaypoints () const
const std::vector< std::string > &	getWaypointNames () const
std::optional< geometry_msgs::msg::Pose >	getNamedPose (std::string name) const
geometry_msgs::msg::Pose	getPose (int index) const
geometry_msgs::msg::Pose	getCurrentPose () const
long	getCurrentWaypointIndex () const
std::optional< std::string >	getCurrentWaypointName () const
void	rewind (int count)
void	forward (int count)
void	seekName (std::string name)
void	loadWaypointsFromYamlParameter (std::string parameter_name, std::string yaml_file_package_name)
void	notifyGoalReached ()
	CpWaypointNavigatorBase ()
virtual	~CpWaypointNavigatorBase ()
void	onInitialize () override
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
void	loadWayPointsFromFile (std::string filepath)
void	loadWayPointsFromFile2 (std::string filepath)
void	setWaypoints (const std::vector< geometry_msgs::msg::Pose > &waypoints)
void	setWaypoints (const std::vector< Pose2D > &waypoints)
const std::vector< geometry_msgs::msg::Pose > &	getWaypoints () const
const std::vector< std::string > &	getWaypointNames () const
std::optional< geometry_msgs::msg::Pose >	getNamedPose (std::string name) const
geometry_msgs::msg::Pose	getPose (int index) const
geometry_msgs::msg::Pose	getCurrentPose () const
long	getCurrentWaypointIndex () const
std::optional< std::string >	getCurrentWaypointName () const
void	rewind (int count)
void	forward (int count)
void	seekName (std::string name)
void	loadWaypointsFromYamlParameter (std::string parameter_name, std::string yaml_file_package_name)
void	notifyGoalReached ()
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Public Attributes
ClNav2Z *	client_
components::CpNav2ActionInterface *	nav2ActionInterface_ = nullptr
smacc2::SmaccSignal< void()>	onNavigationRequestSucceded
smacc2::SmaccSignal< void()>	onNavigationRequestAborted
smacc2::SmaccSignal< void()>	onNavigationRequestCancelled
Public Attributes inherited from cl_nav2z::CpWaypointNavigatorBase
WaypointEventDispatcher	waypointsEventDispatcher
long	currentWaypoint_

Private Member Functions
void	onNavigationResult (const components::CpNav2ActionInterface::WrappedResult &r)
void	onGoalReached (const components::CpNav2ActionInterface::WrappedResult &res)
void	onGoalCancelled (const components::CpNav2ActionInterface::WrappedResult &)
void	onGoalAborted (const components::CpNav2ActionInterface::WrappedResult &)
void	onNavigationResult (const ClNav2Z::WrappedResult &r)
void	onGoalReached (const ClNav2Z::WrappedResult &res)
void	onGoalCancelled (const ClNav2Z::WrappedResult &)
void	onGoalAborted (const ClNav2Z::WrappedResult &)

Private Attributes
boost::signals2::connection	succeddedNav2ZClientConnection_
boost::signals2::connection	abortedNav2ZClientConnection_
boost::signals2::connection	cancelledNav2ZClientConnection_

Additional Inherited Members
Protected Member Functions inherited from cl_nav2z::CpWaypointNavigatorBase
void	insertWaypoint (int index, geometry_msgs::msg::Pose &newpose)
void	removeWaypoint (int index)
void	insertWaypoint (int index, geometry_msgs::msg::Pose &newpose)
void	removeWaypoint (int index)
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 cl_nav2z::CpWaypointNavigatorBase
std::vector< geometry_msgs::msg::Pose >	waypoints_
std::vector< std::string >	waypointsNames_
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 46 of file cp_waypoints_navigator.hpp.

Constructor & Destructor Documentation

CpWaypointNavigator() [1/2]

cl_nav2z::CpWaypointNavigator::CpWaypointNavigator

(

)

Definition at line 44 of file cp_waypoints_navigator.cpp.

{}

CpWaypointNavigator() [2/2]

cl_nav2z::CpWaypointNavigator::CpWaypointNavigator

(

)

Member Function Documentation

onGoalAborted() [1/2]

void cl_nav2z::CpWaypointNavigator::onGoalAborted ( const ClNav2Z::WrappedResult & )

private

Definition at line 55 of file cp_waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestAborted();
}

onGoalAborted() [2/2]

void cl_nav2z::CpWaypointNavigator::onGoalAborted ( const components::CpNav2ActionInterface::WrappedResult & )

private

Definition at line 61 of file cp_waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestAborted();
}

References onNavigationRequestAborted, and stopWaitingResult().

Referenced by onNavigationResult(), and sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

onGoalCancelled() [1/2]

void cl_nav2z::CpWaypointNavigator::onGoalCancelled ( const ClNav2Z::WrappedResult & )

private

Definition at line 48 of file cp_waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestCancelled();
}

onGoalCancelled() [2/2]

void cl_nav2z::CpWaypointNavigator::onGoalCancelled ( const components::CpNav2ActionInterface::WrappedResult & )

private

Definition at line 53 of file cp_waypoints_navigator.cpp.

{
  stopWaitingResult();
 
  this->onNavigationRequestCancelled();
}

References onNavigationRequestCancelled, and stopWaitingResult().

Referenced by onNavigationResult(), and sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

onGoalReached() [1/2]

void cl_nav2z::CpWaypointNavigator::onGoalReached ( const ClNav2Z::WrappedResult & res )

private

Definition at line 62 of file cp_waypoints_navigator.cpp.

{
  waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
  currentWaypoint_++;
  RCLCPP_WARN(
    getLogger(), "[CpWaypointNavigator] Goal result received, incrementing waypoint index: %ld",
    currentWaypoint_);
  stopWaitingResult();
 
  this->notifyGoalReached();
 
  onNavigationRequestSucceded();
}

onGoalReached() [2/2]

void cl_nav2z::CpWaypointNavigator::onGoalReached ( const components::CpNav2ActionInterface::WrappedResult & res )

private

Definition at line 69 of file cp_waypoints_navigator.cpp.

{
  waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
  currentWaypoint_++;
  RCLCPP_WARN(
    getLogger(), "[CpWaypointNavigator] Goal result received, incrementing waypoint index: %ld",
    currentWaypoint_);
  stopWaitingResult();
 
  this->notifyGoalReached();
 
  onNavigationRequestSucceded();
}

References cl_nav2z::CpWaypointNavigatorBase::currentWaypoint_, smacc2::ISmaccComponent::getLogger(), cl_nav2z::CpWaypointNavigatorBase::notifyGoalReached(), onNavigationRequestSucceded, WaypointEventDispatcher::postWaypointEvent(), stopWaitingResult(), and cl_nav2z::CpWaypointNavigatorBase::waypointsEventDispatcher.

Referenced by onNavigationResult(), and sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize() [1/2]

void cl_nav2z::CpWaypointNavigator::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 48 of file cp_waypoints_navigator.cpp.

{
  this->requiresComponent(nav2ActionInterface_, ComponentRequirement::HARD);
}

References nav2ActionInterface_, and smacc2::ISmaccComponent::requiresComponent().

Here is the call graph for this function:

onInitialize() [2/2]

void cl_nav2z::CpWaypointNavigator::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

onNavigationResult() [1/2]

void cl_nav2z::CpWaypointNavigator::onNavigationResult ( const ClNav2Z::WrappedResult & r )

private

Definition at line 308 of file cp_waypoints_navigator.cpp.

{
  if (r.code == rclcpp_action::ResultCode::SUCCEEDED)
  {
    this->onGoalReached(r);
  }
  else if (r.code == rclcpp_action::ResultCode::ABORTED)
  {
    this->onGoalAborted(r);
  }
  else if (r.code == rclcpp_action::ResultCode::CANCELED)
  {
    this->onGoalCancelled(r);
  }
  else
  {
    this->onGoalAborted(r);
  }
}

onNavigationResult() [2/2]

void cl_nav2z::CpWaypointNavigator::onNavigationResult ( const components::CpNav2ActionInterface::WrappedResult & r )

private

Definition at line 329 of file cp_waypoints_navigator.cpp.

{
  if (r.code == rclcpp_action::ResultCode::SUCCEEDED)
  {
    this->onGoalReached(r);
  }
  else if (r.code == rclcpp_action::ResultCode::ABORTED)
  {
    this->onGoalAborted(r);
  }
  else if (r.code == rclcpp_action::ResultCode::CANCELED)
  {
    this->onGoalCancelled(r);
  }
  else
  {
    this->onGoalAborted(r);
  }
}

References onGoalAborted(), onGoalCancelled(), and onGoalReached().

Here is the call graph for this function:

onStateOrthogonalAllocation() [1/2]

template<typename TOrthogonal , typename TSourceObject >

void cl_nav2z::CpWaypointNavigator::onStateOrthogonalAllocation ( )

inline

Definition at line 59 of file cp_waypoints_navigator.hpp.

  {
    client_ = dynamic_cast<ClNav2Z *>(owner_);
    waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
    this->requiresComponent(nav2ActionInterface_, ComponentRequirement::HARD);
  }

References client_, WaypointEventDispatcher::initialize(), smacc2::ISmaccComponent::owner_, smacc2::ISmaccComponent::requiresComponent(), and cl_nav2z::CpWaypointNavigatorBase::waypointsEventDispatcher.

Here is the call graph for this function:

onStateOrthogonalAllocation() [2/2]

template<typename TOrthogonal , typename TSourceObject >

void cl_nav2z::CpWaypointNavigator::onStateOrthogonalAllocation ( )

inline

Definition at line 56 of file cp_waypoints_navigator.hpp.

  {
    waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
  }

References client_, WaypointEventDispatcher::initialize(), and cl_nav2z::CpWaypointNavigatorBase::waypointsEventDispatcher.

Here is the call graph for this function:

sendNextGoal() [1/2]

std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > > cl_nav2z::CpWaypointNavigator::sendNextGoal

(

std::optional< NavigateNextWaypointOptions >

options = std::nullopt

)

Definition at line 200 of file cp_waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size())
  {
    auto & next = waypoints_[currentWaypoint_];
 
    std::string nextName;
    if ((long)waypointsNames_.size() > currentWaypoint_)
    {
      nextName = waypointsNames_[currentWaypoint_];
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %s", nextName.c_str());
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %ld", currentWaypoint_);
    }
 
    nav2_msgs::action::NavigateToPose::Goal goal;
    CpPose * p;
    this->requiresComponent(p, ComponentRequirement::HARD);
    auto pose = p->toPoseMsg();
 
    // configuring goal
    goal.pose.header.frame_id = p->getReferenceFrame();
    //goal.pose.header.stamp = getNode()->now();
    goal.pose.pose = next;
 
    cl_nav2z::CpPlannerSwitcher * plannerSwitcher;
    this->requiresComponent(plannerSwitcher, ComponentRequirement::HARD);
 
    plannerSwitcher->setDefaultPlanners(false);
    if (options && options->controllerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override controller: %s",
        options->controllerName_->c_str());
 
      plannerSwitcher->setDesiredController(*options->controllerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default planners");
    }
 
    cl_nav2z::CpGoalCheckerSwitcher * goalCheckerSwitcher;
    // this->requiresComponent(goalCheckerSwitcher, ComponentRequirement::HARD);
    this->requiresComponent(goalCheckerSwitcher, ComponentRequirement::SOFT);
 
    if (options && options->goalCheckerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override goal checker: %s",
        options->goalCheckerName_->c_str());
 
      goalCheckerSwitcher->setGoalCheckerId(*options->goalCheckerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default goal checker");
      goalCheckerSwitcher->setGoalCheckerId("goal_checker");
    }
 
    plannerSwitcher->commitPublish();
 
    // publish stuff
    // rclcpp::sleep_for(5s);
 
    RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Getting odom tracker");
 
    cl_nav2z::odom_tracker::CpOdomTracker * odomTracker;
    requiresComponent(odomTracker, ComponentRequirement::SOFT);
 
    if (odomTracker != nullptr)
    {
      RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Storing path in odom tracker");
 
      auto pathname = this->owner_->getStateMachine()->getCurrentState()->getName() + " - " +
                      getName() + " - " + nextName;
      odomTracker->pushPath(pathname);
      odomTracker->setStartPoint(pose);
      odomTracker->setWorkingMode(cl_nav2z::odom_tracker::WorkingMode::RECORD_PATH);
    }
 
    // SEND GOAL
    // if (!succeddedNav2ZClientConnection_.connected())
    // {
    //   this->succeddedNav2ZClientConnection_ =
    //     client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
    //   this->cancelledNav2ZClientConnection_ =
    //     client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
    //   this->abortedNav2ZClientConnection_ =
    //     client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
    // }
 
    // Set up navigation result handling
    if (!succeddedNav2ZClientConnection_.connected())
    {
      succeddedNav2ZClientConnection_ =
        nav2ActionInterface_->onNavigationSucceeded(&CpWaypointNavigator::onGoalReached, this);
      abortedNav2ZClientConnection_ =
        nav2ActionInterface_->onNavigationAborted(&CpWaypointNavigator::onGoalAborted, this);
      cancelledNav2ZClientConnection_ =
        nav2ActionInterface_->onNavigationCancelled(&CpWaypointNavigator::onGoalCancelled, this);
    }
 
    return nav2ActionInterface_->sendGoal(goal);
  }
  else
  {
    RCLCPP_WARN(
      getLogger(),
      "[CpWaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
  }
 
  return std::nullopt;
}

References abortedNav2ZClientConnection_, cancelledNav2ZClientConnection_, cl_nav2z::CpPlannerSwitcher::commitPublish(), cl_nav2z::CpWaypointNavigatorBase::currentWaypoint_, smacc2::ISmaccStateMachine::getCurrentState(), smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getName(), smacc2::ISmaccState::getName(), cl_nav2z::CpPose::getReferenceFrame(), smacc2::ISmaccClient::getStateMachine(), nav2ActionInterface_, onGoalAborted(), onGoalCancelled(), onGoalReached(), cl_nav2z::components::CpNav2ActionInterface::onNavigationAborted(), cl_nav2z::components::CpNav2ActionInterface::onNavigationCancelled(), cl_nav2z::components::CpNav2ActionInterface::onNavigationSucceeded(), smacc2::ISmaccComponent::owner_, cl_nav2z::odom_tracker::CpOdomTracker::pushPath(), cl_nav2z::odom_tracker::RECORD_PATH, smacc2::ISmaccComponent::requiresComponent(), cl_nav2z::components::CpNav2ActionInterface::sendGoal(), cl_nav2z::CpPlannerSwitcher::setDefaultPlanners(), cl_nav2z::CpPlannerSwitcher::setDesiredController(), cl_nav2z::CpGoalCheckerSwitcher::setGoalCheckerId(), cl_nav2z::odom_tracker::CpOdomTracker::setStartPoint(), cl_nav2z::odom_tracker::CpOdomTracker::setWorkingMode(), succeddedNav2ZClientConnection_, cl_nav2z::CpPose::toPoseMsg(), cl_nav2z::CpWaypointNavigatorBase::waypoints_, and cl_nav2z::CpWaypointNavigatorBase::waypointsNames_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

sendNextGoal() [2/2]

std::optional< std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > > > cl_nav2z::CpWaypointNavigator::sendNextGoal	(	std::optional< NavigateNextWaypointOptions >	options = std::nullopt,
		cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr	callback = cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::WeakPtr() )

Definition at line 192 of file cp_waypoints_navigator.cpp.

{
  if (currentWaypoint_ >= 0 && currentWaypoint_ < (int)waypoints_.size())
  {
    auto & next = waypoints_[currentWaypoint_];
 
    std::string nextName;
    if ((long)waypointsNames_.size() > currentWaypoint_)
    {
      nextName = waypointsNames_[currentWaypoint_];
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %s", nextName.c_str());
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[CpWaypointNavigator] sending goal, waypoint: %ld", currentWaypoint_);
    }
 
    ClNav2Z::Goal goal;
    auto p = client_->getComponent<cl_nav2z::Pose>();
    auto pose = p->toPoseMsg();
 
    // configuring goal
    goal.pose.header.frame_id = p->getReferenceFrame();
    //goal.pose.header.stamp = getNode()->now();
    goal.pose.pose = next;
 
    auto plannerSwitcher = client_->getComponent<CpPlannerSwitcher>();
    plannerSwitcher->setDefaultPlanners(false);
    if (options && options->controllerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override controller: %s",
        options->controllerName_->c_str());
 
      plannerSwitcher->setDesiredController(*options->controllerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default planners");
    }
 
    auto goalCheckerSwitcher = client_->getComponent<CpGoalCheckerSwitcher>();
 
    if (options && options->goalCheckerName_)
    {
      RCLCPP_WARN(
        getLogger(), "[WaypointsNavigator] override goal checker: %s",
        options->goalCheckerName_->c_str());
 
      goalCheckerSwitcher->setGoalCheckerId(*options->goalCheckerName_);
    }
    else
    {
      RCLCPP_WARN(getLogger(), "[WaypointsNavigator] Configuring default goal checker");
      goalCheckerSwitcher->setGoalCheckerId("goal_checker");
    }
 
    plannerSwitcher->commitPublish();
 
    // publish stuff
    // rclcpp::sleep_for(5s);
 
    RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Getting odom tracker");
    auto odomTracker = client_->getComponent<cl_nav2z::odom_tracker::CpOdomTracker>();
    if (odomTracker != nullptr)
    {
      RCLCPP_INFO(getLogger(), "[WaypointsNavigator] Storing path in odom tracker");
 
      auto pathname = this->owner_->getStateMachine()->getCurrentState()->getName() + " - " +
                      getName() + " - " + nextName;
      odomTracker->pushPath(pathname);
      odomTracker->setStartPoint(pose);
      odomTracker->setWorkingMode(cl_nav2z::odom_tracker::WorkingMode::RECORD_PATH);
    }
 
    // SEND GOAL
    // if (!succeddedNav2ZClientConnection_.connected())
    // {
    //   this->succeddedNav2ZClientConnection_ =
    //     client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
    //   this->cancelledNav2ZClientConnection_ =
    //     client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
    //   this->abortedNav2ZClientConnection_ =
    //     client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
    // }
 
    auto callbackptr = resultCallback.lock();
    succeddedNav2ZClientConnection_ = this->getStateMachine()->createSignalConnection(
      *callbackptr, &CpWaypointNavigator::onNavigationResult, this);
 
    return client_->sendGoal(goal, resultCallback);
  }
  else
  {
    RCLCPP_WARN(
      getLogger(),
      "[CpWaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
  }
 
  return std::nullopt;
}

References cl_nav2z::odom_tracker::RECORD_PATH, cl_nav2z::CpPlannerSwitcher::setDefaultPlanners(), and cl_nav2z::Pose::toPoseMsg().

Here is the call graph for this function:

stopWaitingResult() [1/2]

void cl_nav2z::CpWaypointNavigator::stopWaitingResult

(

)

Definition at line 188 of file cp_waypoints_navigator.cpp.

{
  if (succeddedNav2ZClientConnection_.connected())
  {
    this->succeddedNav2ZClientConnection_.disconnect();
    this->cancelledNav2ZClientConnection_.disconnect();
    this->abortedNav2ZClientConnection_.disconnect();
  }
}

References abortedNav2ZClientConnection_, cancelledNav2ZClientConnection_, and succeddedNav2ZClientConnection_.

Referenced by cl_nav2z::CbNavigateNextWaypoint::onExit(), onGoalAborted(), onGoalCancelled(), and onGoalReached().

Here is the caller graph for this function:

stopWaitingResult() [2/2]

void cl_nav2z::CpWaypointNavigator::stopWaitingResult

(

)

Member Data Documentation

abortedNav2ZClientConnection_

boost::signals2::connection cl_nav2z::CpWaypointNavigator::abortedNav2ZClientConnection_

private

Definition at line 84 of file cp_waypoints_navigator.hpp.

Referenced by sendNextGoal(), and stopWaitingResult().

cancelledNav2ZClientConnection_

boost::signals2::connection cl_nav2z::CpWaypointNavigator::cancelledNav2ZClientConnection_

private

Definition at line 85 of file cp_waypoints_navigator.hpp.

Referenced by sendNextGoal(), and stopWaitingResult().

client_

ClNav2Z * cl_nav2z::CpWaypointNavigator::client_

Definition at line 51 of file cp_waypoints_navigator.hpp.

Referenced by onStateOrthogonalAllocation().

nav2ActionInterface_

components::CpNav2ActionInterface* cl_nav2z::CpWaypointNavigator::nav2ActionInterface_ = nullptr

Definition at line 52 of file cp_waypoints_navigator.hpp.

Referenced by onInitialize(), and sendNextGoal().

onNavigationRequestAborted

smacc2::SmaccSignal< void()> cl_nav2z::CpWaypointNavigator::onNavigationRequestAborted

Definition at line 73 of file cp_waypoints_navigator.hpp.

Referenced by onGoalAborted().

onNavigationRequestCancelled

smacc2::SmaccSignal< void()> cl_nav2z::CpWaypointNavigator::onNavigationRequestCancelled

Definition at line 74 of file cp_waypoints_navigator.hpp.

Referenced by onGoalCancelled().

onNavigationRequestSucceded

smacc2::SmaccSignal< void()> cl_nav2z::CpWaypointNavigator::onNavigationRequestSucceded

Definition at line 72 of file cp_waypoints_navigator.hpp.

Referenced by onGoalReached().

succeddedNav2ZClientConnection_

boost::signals2::connection cl_nav2z::CpWaypointNavigator::succeddedNav2ZClientConnection_

private

Definition at line 83 of file cp_waypoints_navigator.hpp.

Referenced by sendNextGoal(), and stopWaitingResult().

---

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

• smacc2_client_library/cl_nav2z/include/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.hpp
• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp
• smacc2_client_library/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.cpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.cpp