cl_nav2z::CbPureSpinning Struct Reference

#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
	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 ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
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)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

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() [1/2]

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)
{
}

CbPureSpinning() [2/2]

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

Member Function Documentation

onEntry() [1/2]

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::CpPose * pose;
  this->requiresComponent(pose, ComponentRequirement::HARD);
 
  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:

onEntry() [2/2]

void cl_nav2z::CbPureSpinning::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

onExit() [1/2]

void cl_nav2z::CbPureSpinning::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 101 of file cb_pure_spinning.cpp.

{}

onExit() [2/2]

void cl_nav2z::CbPureSpinning::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

updateParameters() [1/2]

void cl_nav2z::CbPureSpinning::updateParameters

(

)

Definition at line 37 of file cb_pure_spinning.cpp.

{}

updateParameters() [2/2]

void cl_nav2z::CbPureSpinning::updateParameters

(

)

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/cl_nav2z/include/cl_nav2z/client_behaviors/cb_pure_spinning.hpp
• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp
• smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_pure_spinning.cpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp