.. _program_listing_file__tmp_ws_src_SMACC2_smacc2_sm_reference_library_sm_dance_bot_warehouse_3_include_sm_dance_bot_warehouse_3_clients_cl_nav2z_behaviors_cb_pure_spinning.hpp:

Program Listing for File cb_pure_spinning.hpp
=============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_SMACC2_smacc2_sm_reference_library_sm_dance_bot_warehouse_3_include_sm_dance_bot_warehouse_3_clients_cl_nav2z_behaviors_cb_pure_spinning.hpp>` (``/tmp/ws/src/SMACC2/smacc2_sm_reference_library/sm_dance_bot_warehouse_3/include/sm_dance_bot_warehouse_3/clients/cl_nav2z/behaviors/cb_pure_spinning.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2021 RobosoftAI Inc.
   //
   // Licensed under the Apache License, Version 2.0 (the "License");
   // you may not use this file except in compliance with the License.
   // You may obtain a copy of the License at
   //
   //     http://www.apache.org/licenses/LICENSE-2.0
   //
   // Unless required by applicable law or agreed to in writing, software
   // distributed under the License is distributed on an "AS IS" BASIS,
   // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   // See the License for the specific language governing permissions and
   // limitations under the License.
   
   /*****************************************************************************************************************
    *
    *   Authors: Pablo Inigo Blasco, Brett Aldrich
    *
    ******************************************************************************************************************/
   
   #pragma once
   
   #include <multirole_sensor_client/client_behaviors/cb_default_multirole_sensor_behavior.hpp>
   #include <sensor_msgs/msg/laser_scan.hpp>
   #include <sm_dance_bot_warehouse_3/clients/cl_lidar/cl_lidar.hpp>
   #include <geometry_msgs/msg/twist.hpp>
   #include <nav2z_client/components/pose/cp_pose.hpp>
   #include <angles/angles.h>
   
   namespace sm_dance_bot_warehouse_3
   {
   namespace cl_nav2zclient
   {
   struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior
   {
     private:
       double targetYaw_;
       bool goalReached_;
       double k_betta_;
       double max_angular_yaw_speed_;
   
       rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
   
     public:
       double yaw_goal_tolerance_rads_;
   
     CbPureSpinning(double targetYaw, double max_angular_yaw_speed= 0.5)
     : targetYaw_(targetYaw),
       k_betta_(1.0),
       max_angular_yaw_speed_(max_angular_yaw_speed),
       yaw_goal_tolerance_rads_(0.03)
     {
   
     }
   
     void updateParameters()
     {
     }
   
     void onEntry() override
     {
         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::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();
     }
   
     void onExit() override
     {
     }
     };
   }
   }