#include <cb_rotate.hpp>

Inheritance diagram for cl_nitrosz::CbRotate:

Collaboration diagram for cl_nitrosz::CbRotate:

Public Member Functions
	CbRotate (float rotate_degree)

	CbRotate (float rotate_degree, cl_nitrosz::SpinningPlanner spinning_planner)

void	onEntry () override

Public Member Functions inherited from cl_nitrosz::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_nitrosz::SpinningPlanner >	spinningPlanner

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

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

void	cancelGoal ()

bool	isOwnActionResponse (const ClNitrosZ::WrappedResult &)

virtual void	onNavigationResult (const ClNitrosZ::WrappedResult &)

virtual void	onNavigationActionSuccess (const ClNitrosZ::WrappedResult &)

virtual void	onNavigationActionAbort (const ClNitrosZ::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_nitrosz::CbNav2ZClientBehaviorBase
cl_nitrosz::ClNitrosZ *	nitroszClient_

cl_nitrosz::ClNitrosZ::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_nitrosz::CbRotate::CbRotate ( float rotate_degree )

Definition at line 29 of file cb_rotate.cpp.

29: rotateDegree(rotate_degree) {}

cl_nitrosz::CbRotate::rotateDegree

float rotateDegree

Definition cb_rotate.hpp:31

◆ CbRotate() [2/2]

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

Definition at line 31 of file cb_rotate.cpp.

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

Member Function Documentation

◆ onEntry()

void cl_nitrosz::CbRotate::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 36 of file cb_rotate.cpp.

{
  double angle_increment_degree = rotateDegree;
 
  auto plannerSwitcher = nitroszClient_->getComponent<CpPlannerSwitcher>();
 
  if (spinningPlanner && *spinningPlanner == SpinningPlanner::PureSpinning)
  {
    plannerSwitcher->setPureSpinningPlanner();
  }
  else
  {
    plannerSwitcher->setDefaultPlanners();
  }
 
  auto p = nitroszClient_->getComponent<cl_nitrosz::Pose>();
  auto referenceFrame = p->getReferenceFrame();
  auto currentPoseMsg = p->toPoseMsg();
 
  tf2::Transform currentPose;
  tf2::fromMsg(currentPoseMsg, currentPose);
 
  auto odomTracker = nitroszClient_->getComponent<odom_tracker::CpOdomTracker>();
  ClNitrosZ::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(nitroszClient_);
  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_nitrosz::Pose::getReferenceFrame(), cl_nitrosz::CbNav2ZClientBehaviorBase::nitroszClient_, cl_nitrosz::PureSpinning, cl_nitrosz::odom_tracker::RECORD_PATH, smacc2::ISmaccClientBehavior::requiresClient(), rotateDegree, cl_nitrosz::CbNav2ZClientBehaviorBase::sendGoal(), cl_nitrosz::CpPlannerSwitcher::setPureSpinningPlanner(), and spinningPlanner.

Here is the call graph for this function:

Member Data Documentation

◆ goalChecker_

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

Definition at line 33 of file cb_rotate.hpp.

◆ listener

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

private

Definition at line 45 of file cb_rotate.hpp.

◆ rotateDegree

float cl_nitrosz::CbRotate::rotateDegree

Definition at line 31 of file cb_rotate.hpp.

Referenced by onEntry().

◆ spinningPlanner

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

Definition at line 35 of file cb_rotate.hpp.