#include <cb_rotate.hpp>

Inheritance diagram for cl_nav2z::CbRotate:

Collaboration diagram for cl_nav2z::CbRotate:

Public Member Functions
	CbRotate (float rotate_degree)

	CbRotate (float rotate_degree, cl_nav2z::SpinningPlanner spinning_planner)

void	onEntry () override

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

Public Attributes
float	rotateDegree

std::optional< std::string >	goalChecker_

std::optional< cl_nav2z::SpinningPlanner >	spinningPlanner

Private Attributes
std::shared_ptr< tf2_ros::Buffer >	listener

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 ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.

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 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 28 of file cb_rotate.hpp.

Constructor & Destructor Documentation

◆ CbRotate() [1/2]

cl_nav2z::CbRotate::CbRotate ( float rotate_degree )

Definition at line 29 of file cb_rotate.cpp.

29: rotateDegree(rotate_degree) {}

cl_nav2z::CbRotate::rotateDegree

float rotateDegree

Definition cb_rotate.hpp:31

◆ CbRotate() [2/2]

cl_nav2z::CbRotate::CbRotate	(	float	rotate_degree,
		cl_nav2z::SpinningPlanner	spinning_planner
	)

Definition at line 31 of file cb_rotate.cpp.

: rotateDegree(rotate_degree), spinningPlanner(spinning_planner)
{
}

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbRotate::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 36 of file cb_rotate.cpp.

{
  double angle_increment_degree = rotateDegree;
 
  auto plannerSwitcher = nav2zClient_->getComponent<CpPlannerSwitcher>();
 
  if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
  {
    plannerSwitcher->setPureSpinningPlanner();
  }
  else
  {
    plannerSwitcher->setDefaultPlanners();
  }
 
  auto p = nav2zClient_->getComponent<cl_nav2z::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  tf2::Transform currentPose;
  tf2::fromMsg(currentPoseMsg, currentPose);
 
  auto odomTracker = nav2zClient_->getComponent<odom_tracker::CpOdomTracker>();
  ClNav2Z::Goal goal;
  goal.pose.header.frame_id = referenceFrame;
  //goal.pose.header.stamp = getNode()->now();
 
  auto currentAngle = tf2::getYaw(currentPoseMsg.orientation);
  auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0;
  goal.pose.pose.position = currentPoseMsg.position;
  tf2::Quaternion q;
  q.setRPY(0, 0, targetAngle);
  goal.pose.pose.orientation = tf2::toMsg(q);
 
  geometry_msgs::msg::PoseStamped stampedCurrentPoseMsg;
  stampedCurrentPoseMsg.header.frame_id = referenceFrame;
  stampedCurrentPoseMsg.header.stamp = getNode()->now();
  stampedCurrentPoseMsg.pose = currentPoseMsg;
 
  this->requiresClient(nav2zClient_);
  auto pathname = this->getCurrentState()->getName() + " - " + getName();
  odomTracker->pushPath(pathname);
 
  odomTracker->setStartPoint(stampedCurrentPoseMsg);
  odomTracker->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
 
  RCLCPP_INFO_STREAM(getLogger(), "current pose: " << currentPoseMsg);
  RCLCPP_INFO_STREAM(getLogger(), "goal pose: " << goal.pose.pose);
  this->sendGoal(goal);
}

References smacc2::ISmaccClient::getComponent(), smacc2::ISmaccClientBehavior::getCurrentState(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccState::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_nav2z::Pose::getReferenceFrame(), cl_nav2z::CbNav2ZClientBehaviorBase::nav2zClient_, cl_nav2z::PureSpinning, cl_nav2z::odom_tracker::RECORD_PATH, smacc2::ISmaccClientBehavior::requiresClient(), rotateDegree, cl_nav2z::CbNav2ZClientBehaviorBase::sendGoal(), cl_nav2z::CpPlannerSwitcher::setPureSpinningPlanner(), and spinningPlanner.

Here is the call graph for this function:

Member Data Documentation

◆ goalChecker_

std::optional<std::string> cl_nav2z::CbRotate::goalChecker_

Definition at line 33 of file cb_rotate.hpp.

◆ listener

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::CbRotate::listener

private

Definition at line 45 of file cb_rotate.hpp.

◆ rotateDegree

float cl_nav2z::CbRotate::rotateDegree

Definition at line 31 of file cb_rotate.hpp.

Referenced by onEntry().

◆ spinningPlanner

std::optional<cl_nav2z::SpinningPlanner> cl_nav2z::CbRotate::spinningPlanner

Definition at line 35 of file cb_rotate.hpp.