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

virtual void	onExit ()

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

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

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 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<PlannerSwitcher>();
 
  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::OdomTracker>();
  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::PlannerSwitcher::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.