sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning Struct Reference

#include <cb_pure_spinning.hpp>

Inheritance diagram for sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning:

Collaboration diagram for sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning:

Public Member Functions
	CbPureSpinning (double targetYaw, 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)
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)

Public Attributes
double	yaw_goal_tolerance_rads_

Private Attributes
double	targetYaw_
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 ()
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
virtual void	onEntry ()
virtual void	onExit ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual void	dispose ()
virtual rclcpp::Node::SharedPtr	getNode ()
virtual rclcpp::Logger	getLogger ()

Detailed Description

Definition at line 34 of file cb_pure_spinning.hpp.

Constructor & Destructor Documentation

CbPureSpinning()

sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::CbPureSpinning ( double  targetYaw, double  max_angular_yaw_speed = 0.5  )

inline

Definition at line 47 of file cb_pure_spinning.hpp.

  : targetYaw_(targetYaw),
    k_betta_(1.0),
    max_angular_yaw_speed_(max_angular_yaw_speed),
    yaw_goal_tolerance_rads_(0.03)
  {
 
  }

Member Function Documentation

onEntry()

void sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::onEntry ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 60 of file cb_pure_spinning.hpp.

  {
      auto nh = this->getNode();
      cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
 
      cl_nav2z::Pose* pose;
      this->requiresComponent(pose);
 
      geometry_msgs::msg::Twist cmd_vel;
      goalReached_ = false;
 
      geometry_msgs::msg::PoseStamped currentPose = pose->toPoseStampedMsg();
 
      rclcpp::Rate loop_rate(10);
      double countAngle = 0;
      auto prevyaw = tf2::getYaw(currentPose.pose.orientation);
      while(rclcpp::ok() && !goalReached_)
      {
        tf2::Quaternion q;
        currentPose = pose->toPoseStampedMsg();
        tf2::fromMsg(currentPose.pose.orientation, q);
        auto currentYaw = tf2::getYaw(currentPose.pose.orientation);
        auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw);
        countAngle += deltaAngle;
 
        prevyaw = currentYaw;
        double angular_error = targetYaw_ - 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 smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), goalReached_, k_betta_, max_angular_yaw_speed_, smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), smacc2::ISmaccClientBehavior::requiresComponent(), targetYaw_, and yaw_goal_tolerance_rads_.

Here is the call graph for this function:

onExit()

void sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::onExit ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 124 of file cb_pure_spinning.hpp.

  {
  }

updateParameters()

void sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::updateParameters ( )

inline

Definition at line 56 of file cb_pure_spinning.hpp.

  {
  }

Member Data Documentation

cmd_vel_pub_

rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::cmd_vel_pub_

private

Definition at line 42 of file cb_pure_spinning.hpp.

goalReached_

bool sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::goalReached_

private

Definition at line 38 of file cb_pure_spinning.hpp.

Referenced by onEntry().

k_betta_

double sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::k_betta_

private

Definition at line 39 of file cb_pure_spinning.hpp.

Referenced by onEntry().

max_angular_yaw_speed_

double sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::max_angular_yaw_speed_

private

Definition at line 40 of file cb_pure_spinning.hpp.

Referenced by onEntry().

targetYaw_

double sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::targetYaw_

private

Definition at line 37 of file cb_pure_spinning.hpp.

Referenced by onEntry().

yaw_goal_tolerance_rads_

double sm_dance_bot_warehouse_3::cl_nav2zclient::CbPureSpinning::yaw_goal_tolerance_rads_

Definition at line 45 of file cb_pure_spinning.hpp.

Referenced by onEntry().

---

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

• smacc2_sm_reference_library/sm_dance_bot_warehouse_3/include/sm_dance_bot_warehouse_3/clients/cl_nav2z/behaviors/cb_pure_spinning.hpp