#include <cb_navigate_global_position.hpp>

Inheritance diagram for cl_nav2z::CbNavigateGlobalPosition:

Collaboration diagram for cl_nav2z::CbNavigateGlobalPosition:

Public Member Functions
	CbNavigateGlobalPosition ()

	CbNavigateGlobalPosition (float x, float y, float yaw, std::optional< CbNavigateGlobalPositionOptions > options=std::nullopt)

void	setGoal (const geometry_msgs::msg::Pose &pose)

virtual void	onEntry () override

void	onExit () override

void	execute ()

Public Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
virtual	~CbNav2ZClientBehaviorBase ()

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

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

virtual	~SmaccAsyncClientBehavior ()

template<typename TCallback , typename T >
boost::signals2::connection	onSuccess (TCallback callback, T *object)

template<typename TCallback , typename T >
boost::signals2::connection	onFinished (TCallback callback, T *object)

template<typename TCallback , typename T >
boost::signals2::connection	onFailure (TCallback callback, T *object)

void	requestForceFinish ()

void	executeOnEntry () override

void	executeOnExit () override

void	waitOnEntryThread (bool requestFinish)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)

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

virtual	~ISmaccClientBehavior ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

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

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)

virtual void	onEntry ()

virtual void	onExit ()

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

Public Attributes
float	goalYaw

CbNavigateGlobalPositionOptions	options

geometry_msgs::msg::Point	goalPosition

Private Member Functions
void	readStartPoseFromParameterServer (ClNav2Z::Goal &goal)

Additional Inherited Members
Protected Member Functions inherited from cl_nav2z::CbNav2ZClientBehaviorBase
void	sendGoal (ClNav2Z::Goal &goal)

void	cancelGoal ()

bool	isOwnActionResponse (const ClNav2Z::WrappedResult &)

virtual void	onNavigationResult (const ClNav2Z::WrappedResult &)

virtual void	onNavigationActionSuccess (const ClNav2Z::WrappedResult &)

virtual void	onNavigationActionAbort (const ClNav2Z::WrappedResult &)

Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()

void	postFailureEvent ()

virtual void	dispose () override

bool	isShutdownRequested ()

Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()

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

template<typename EventType >
void	postEvent ()

ISmaccState *	getCurrentState ()

virtual void	dispose ()

virtual rclcpp::Node::SharedPtr	getNode () const

virtual rclcpp::Logger	getLogger () const

Protected Attributes inherited from cl_nav2z::CbNav2ZClientBehaviorBase
cl_nav2z::ClNav2Z *	nav2zClient_

cl_nav2z::ClNav2Z::SmaccNavigateResultSignal::SharedPtr	navigationCallback_

rclcpp_action::ResultCode	navigationResult_

std::shared_future< std::shared_ptr< rclcpp_action::ClientGoalHandle< nav2_msgs::action::NavigateToPose > > >	goalHandleFuture_

Detailed Description

Definition at line 40 of file cb_navigate_global_position.hpp.

Constructor & Destructor Documentation

◆ CbNavigateGlobalPosition() [1/2]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition ( )

Definition at line 32 of file cb_navigate_global_position.cpp.

32{}

◆ CbNavigateGlobalPosition() [2/2]

cl_nav2z::CbNavigateGlobalPosition::CbNavigateGlobalPosition	(	float	x,
		float	y,
		float	yaw,
		std::optional< CbNavigateGlobalPositionOptions >	options = `std::nullopt`
	)

Definition at line 34 of file cb_navigate_global_position.cpp.

{
  auto p = geometry_msgs::msg::Point();
  p.x = x;
  p.y = y;
  goalPosition = p;
  goalYaw = yaw;
 
  if (options) this->options = *options;
}

References goalPosition, goalYaw, and options.

Member Function Documentation

◆ execute()

void cl_nav2z::CbNavigateGlobalPosition::execute ( )

Definition at line 83 of file cb_navigate_global_position.cpp.

{
  auto p = nav2zClient_->getComponent<cl_nav2z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  // auto currentPoseMsg = p->toPoseMsg();
 
  RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase");
  ClNav2Z::Goal goal;
  goal.pose.header.frame_id = referenceFrame;
  //goal.pose.header.stamp = getNode()->now();
 
  goal.pose.pose.position = goalPosition;
  tf2::Quaternion q;
  q.setRPY(0, 0, goalYaw);
  goal.pose.pose.orientation = tf2::toMsg(q);
 
  this->sendGoal(goal);
}

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getLogger(), cl_nav2z::Pose::getReferenceFrame(), goalPosition, goalYaw, cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, and cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal().

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onEntry()

void cl_nav2z::CbNavigateGlobalPosition::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbStopNavigation.

Definition at line 52 of file cb_navigate_global_position.cpp.

{
  RCLCPP_INFO(getLogger(), "Entering Navigate Global position");
  RCLCPP_INFO(getLogger(), "Component requirements completed");
 
  auto pose = nav2zClient_->getComponent<cl_nav2z::Pose>()->toPoseMsg();
  auto * odomTracker = nav2zClient_->getComponent<OdomTracker>();
 
  auto plannerSwitcher = nav2zClient_->getComponent<PlannerSwitcher>();
 
  plannerSwitcher->setDefaultPlanners(false);
 
  if (options.controllerName_)
  {
    plannerSwitcher->setDesiredController(*(options.controllerName_));
  }
 
  plannerSwitcher->commitPublish();
 
  auto goalCheckerSwitcher = nav2zClient_->getComponent<GoalCheckerSwitcher>();
  goalCheckerSwitcher->setGoalCheckerId("goal_checker");
 
  auto pathname = this->getCurrentState()->getName() + " - " + getName();
  odomTracker->pushPath(pathname);
  odomTracker->setStartPoint(pose);
  odomTracker->setWorkingMode(WorkingMode::RECORD_PATH);
 
  execute();
}

References cl_nav2z::CbNavigateGlobalPositionOptions::controllerName_, execute(), smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, options, and cl_nav2z::GoalCheckerSwitcher::setGoalCheckerId().

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onExit()

void cl_nav2z::CbNavigateGlobalPosition::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_nav2z::CbStopNavigation.

Definition at line 104 of file cb_navigate_global_position.cpp.

{
  RCLCPP_INFO(getLogger(), "Exiting move goal Action Client");
}

References smacc2::ISmaccClientBehavior::getLogger().

Here is the call graph for this function:

◆ readStartPoseFromParameterServer()

void cl_nav2z::CbNavigateGlobalPosition::readStartPoseFromParameterServer ( ClNav2Z::Goal & goal )

private

◆ setGoal()

void cl_nav2z::CbNavigateGlobalPosition::setGoal ( const geometry_msgs::msg::Pose & pose )

Definition at line 46 of file cb_navigate_global_position.cpp.

{
  goalPosition = pose.position;
  goalYaw = tf2::getYaw(pose.orientation);
}

References goalPosition, and goalYaw.

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

Here is the caller graph for this function:

Member Data Documentation

◆ goalPosition

geometry_msgs::msg::Point cl_nav2z::CbNavigateGlobalPosition::goalPosition

Definition at line 46 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), execute(), and setGoal().

◆ goalYaw

float cl_nav2z::CbNavigateGlobalPosition::goalYaw

Definition at line 43 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), execute(), and setGoal().

◆ options

CbNavigateGlobalPositionOptions cl_nav2z::CbNavigateGlobalPosition::options

Definition at line 45 of file cb_navigate_global_position.hpp.

Referenced by CbNavigateGlobalPosition(), and onEntry().

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

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_global_position.hpp
smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp

Public Member Functions

Public Attributes

Private Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbNavigateGlobalPosition() [1/2]

◆ CbNavigateGlobalPosition() [2/2]

Member Function Documentation

◆ execute()

◆ onEntry()

◆ onExit()

◆ readStartPoseFromParameterServer()

◆ setGoal()

Member Data Documentation

◆ goalPosition

◆ goalYaw

◆ options