#include <cb_pure_spinning.hpp>

Inheritance diagram for cl_nav2z::CbPureSpinning:

Collaboration diagram for cl_nav2z::CbPureSpinning:

Public Member Functions
	CbPureSpinning (double targetYaw_rads, double max_angular_yaw_speed=0.5)

void	updateParameters ()

void	onEntry () override

void	onExit () override

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)

Public Attributes
double	yaw_goal_tolerance_rads_

Private Attributes
double	targetYaw__rads

bool	goalReached_

double	k_betta_

double	max_angular_yaw_speed_

rclcpp::Publisher< geometry_msgs::msg::Twist >::SharedPtr	cmd_vel_pub_

Additional Inherited Members
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

Detailed Description

Definition at line 28 of file cb_pure_spinning.hpp.

Constructor & Destructor Documentation

◆ CbPureSpinning()

cl_nav2z::CbPureSpinning::CbPureSpinning	(	double	targetYaw_rads,
		double	max_angular_yaw_speed = `0.5`
	)

Definition at line 29 of file cb_pure_spinning.cpp.

: targetYaw__rads(targetYaw_rads),
  k_betta_(1.0),
  max_angular_yaw_speed_(max_angular_yaw_speed),
  yaw_goal_tolerance_rads_(0.1)
{
}

Member Function Documentation

◆ onEntry()

void cl_nav2z::CbPureSpinning::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 39 of file cb_pure_spinning.cpp.

{
  auto nh = this->getNode();
  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
 
  cl_nav2z::Pose * pose;
  this->requiresComponent(pose);
 
  geometry_msgs::msg::Twist cmd_vel;
  goalReached_ = false;
 
  geometry_msgs::msg::Pose currentPose = pose->toPoseMsg();
 
  rclcpp::Rate loop_rate(10);
  double countAngle = 0;
  auto prevyaw = tf2::getYaw(currentPose.orientation);
  while (rclcpp::ok() && !goalReached_)
  {
    tf2::Quaternion q;
    currentPose = pose->toPoseMsg();
    tf2::fromMsg(currentPose.orientation, q);
    auto currentYaw = tf2::getYaw(currentPose.orientation);
    auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw);
    countAngle += deltaAngle;
 
    prevyaw = currentYaw;
    double angular_error = targetYaw__rads - countAngle;
 
    auto omega = angular_error * k_betta_;
    cmd_vel.linear.x = 0;
    cmd_vel.linear.y = 0;
    cmd_vel.linear.z = 0;
    cmd_vel.angular.z =
      std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_));
 
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " << deltaAngle);
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " << countAngle);
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] k_betta_: " << k_betta_);
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] angular error: " << angular_error << "("
                       << yaw_goal_tolerance_rads_ << ")");
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] command angular speed: " << cmd_vel.angular.z);
 
    if (fabs(angular_error) < yaw_goal_tolerance_rads_)
    {
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. Sending stop command.");
      goalReached_ = true;
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = 0;
      break;
    }
 
    this->cmd_vel_pub_->publish(cmd_vel);
 
    loop_rate.sleep();
  }
 
  this->postSuccessEvent();
}

References cmd_vel_pub_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), goalReached_, k_betta_, max_angular_yaw_speed_, smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), smacc2::ISmaccClientBehavior::requiresComponent(), targetYaw__rads, and yaw_goal_tolerance_rads_.

Here is the call graph for this function:

◆ onExit()

void cl_nav2z::CbPureSpinning::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 101 of file cb_pure_spinning.cpp.

101{}

◆ updateParameters()

void cl_nav2z::CbPureSpinning::updateParameters ( )

Definition at line 37 of file cb_pure_spinning.cpp.

37{}

Member Data Documentation

◆ cmd_vel_pub_

rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cl_nav2z::CbPureSpinning::cmd_vel_pub_

private

Definition at line 36 of file cb_pure_spinning.hpp.

Referenced by onEntry().

◆ goalReached_

bool cl_nav2z::CbPureSpinning::goalReached_

private

Definition at line 32 of file cb_pure_spinning.hpp.

Referenced by onEntry().

◆ k_betta_

double cl_nav2z::CbPureSpinning::k_betta_

private

Definition at line 33 of file cb_pure_spinning.hpp.

Referenced by onEntry().

◆ max_angular_yaw_speed_

double cl_nav2z::CbPureSpinning::max_angular_yaw_speed_

private

Definition at line 34 of file cb_pure_spinning.hpp.

Referenced by onEntry().

◆ targetYaw__rads

double cl_nav2z::CbPureSpinning::targetYaw__rads

private

Definition at line 31 of file cb_pure_spinning.hpp.

Referenced by onEntry().

◆ yaw_goal_tolerance_rads_

double cl_nav2z::CbPureSpinning::yaw_goal_tolerance_rads_

Definition at line 39 of file cb_pure_spinning.hpp.

Referenced by onEntry().

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

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

Public Member Functions

Public Attributes

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbPureSpinning()

Member Function Documentation

◆ onEntry()

◆ onExit()

◆ updateParameters()

Member Data Documentation

◆ cmd_vel_pub_

◆ goalReached_

◆ k_betta_

◆ max_angular_yaw_speed_

◆ targetYaw__rads

◆ yaw_goal_tolerance_rads_