#include <cb_position_control_free_space.hpp>

Inheritance diagram for cl_nitrosz::CbPositionControlFreeSpace:

Collaboration diagram for cl_nitrosz::CbPositionControlFreeSpace:

Public Member Functions
	CbPositionControlFreeSpace ()

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_ = 0.1

double	threshold_distance_ = 3.0

geometry_msgs::msg::Pose	target_pose_

Private Attributes
double	targetYaw_

bool	goalReached_

double	k_betta_

double	max_angular_yaw_speed_

double	prev_error_linear_ = 0.0

double	prev_error_angular_ = 0.0

double	integral_linear_ = 0.0

double	integral_angular_ = 0.0

double	max_linear_velocity = 1.0

double	max_angular_velocity = 1.0

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 29 of file cb_position_control_free_space.hpp.

Constructor & Destructor Documentation

◆ CbPositionControlFreeSpace()

cl_nitrosz::CbPositionControlFreeSpace::CbPositionControlFreeSpace ( )

Definition at line 30 of file cb_position_control_free_space.cpp.

: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0)
{
}

Member Function Documentation

◆ onEntry()

void cl_nitrosz::CbPositionControlFreeSpace::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 37 of file cb_position_control_free_space.cpp.

{
  auto nh = this->getNode();
  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
 
  cl_nitrosz::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);
 
  // PID controller gains (proportional, integral, and derivative)
  double kp_linear = 0.5;
  double ki_linear = 0.0;
  double kd_linear = 0.1;
 
  double kp_angular = 0.5;
  double ki_angular = 0.0;
  double kd_angular = 0.1;
 
  while (rclcpp::ok() && !goalReached_)
  {
    RCLCPP_INFO_STREAM_THROTTLE(
      getLogger(), *getNode()->get_clock(), 200,
      "CbPositionControlFreeSpace, current pose: " << currentPose.position.x << ", "
                                                   << currentPose.position.y << ", "
                                                   << tf2::getYaw(currentPose.orientation));
 
    RCLCPP_INFO_STREAM_THROTTLE(
      getLogger(), *getNode()->get_clock(), 200,
      "CbPositionControlFreeSpace, target pose: " << target_pose_.position.x << ", "
                                                  << target_pose_.position.y << ", "
                                                  << tf2::getYaw(target_pose_.orientation));
 
    tf2::Quaternion q;
    currentPose = pose->toPoseMsg();
 
    // Here we implement the control logic to calculate the velocity command
    // based on the received position of the vehicle and the target pose.
 
    // Calculate the errors in x and y
    double error_x = target_pose_.position.x - currentPose.position.x;
    double error_y = target_pose_.position.y - currentPose.position.y;
 
    // Calculate the distance to the target pose
    double distance_to_target = std::sqrt(error_x * error_x + error_y * error_y);
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] distance to target: " << distance_to_target
                       << " ( th: " << threshold_distance_ << ")");
 
    // Check if the robot has reached the target pose
    if (distance_to_target < threshold_distance_)
    {
      RCLCPP_INFO(getLogger(), "Goal reached!");
      // Stop the robot by setting the velocity commands to zero
      geometry_msgs::msg::Twist cmd_vel_msg;
      cmd_vel_msg.linear.x = 0.0;
      cmd_vel_msg.angular.z = 0.0;
      cmd_vel_pub_->publish(cmd_vel_msg);
      break;
    }
    else
    {
      // Calculate the desired orientation angle
      double desired_yaw = std::atan2(error_y, error_x);
 
      // Calculate the difference between the desired orientation and the
      // current orientation
      double yaw_error = desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI);
 
      // Ensure the yaw error is within the range [-pi, pi]
      while (yaw_error > M_PI) yaw_error -= 2 * M_PI;
      while (yaw_error < -M_PI) yaw_error += 2 * M_PI;
 
      // Calculate the control signals (velocity commands) using PID controllers
      double cmd_linear_x = kp_linear * distance_to_target + ki_linear * integral_linear_ +
                            kd_linear * (distance_to_target - prev_error_linear_);
      double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ +
                             kd_angular * (yaw_error - prev_error_angular_);
 
      if (cmd_linear_x > max_linear_velocity)
        cmd_linear_x = max_linear_velocity;
      else if (cmd_linear_x < -max_linear_velocity)
        cmd_linear_x = -max_linear_velocity;
 
      if (cmd_angular_z > max_angular_velocity)
        cmd_angular_z = max_angular_velocity;
      else if (cmd_angular_z < -max_angular_velocity)
        cmd_angular_z = -max_angular_velocity;
 
      // Construct and publish the velocity command message
      geometry_msgs::msg::Twist cmd_vel_msg;
 
      cmd_vel_msg.linear.x = cmd_linear_x;
      cmd_vel_msg.angular.z = cmd_angular_z;
 
      cmd_vel_pub_->publish(cmd_vel_msg);
 
      // Update errors and integrals for the next control cycle
      prev_error_linear_ = distance_to_target;
      prev_error_angular_ = yaw_error;
      integral_linear_ += distance_to_target;
      integral_angular_ += yaw_error;
 
      // 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_ - 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();
    }
  }
 
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution");
 
  this->postSuccessEvent();
}

References cmd_vel_pub_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), goalReached_, integral_angular_, integral_linear_, max_angular_velocity, max_linear_velocity, smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), prev_error_angular_, prev_error_linear_, smacc2::ISmaccClientBehavior::requiresComponent(), target_pose_, and threshold_distance_.

Referenced by cl_nitrosz::CbNavigateNextWaypointFree::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onExit()

void cl_nitrosz::CbPositionControlFreeSpace::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 197 of file cb_position_control_free_space.cpp.

197{}

◆ updateParameters()

void cl_nitrosz::CbPositionControlFreeSpace::updateParameters ( )

Definition at line 35 of file cb_position_control_free_space.cpp.

35{}

Member Data Documentation

◆ cmd_vel_pub_

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

private

Definition at line 47 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ goalReached_

bool cl_nitrosz::CbPositionControlFreeSpace::goalReached_

private

Definition at line 33 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ integral_angular_

double cl_nitrosz::CbPositionControlFreeSpace::integral_angular_ = 0.0

private

Definition at line 40 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ integral_linear_

double cl_nitrosz::CbPositionControlFreeSpace::integral_linear_ = 0.0

private

Definition at line 39 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ k_betta_

double cl_nitrosz::CbPositionControlFreeSpace::k_betta_

private

Definition at line 34 of file cb_position_control_free_space.hpp.

◆ max_angular_velocity

double cl_nitrosz::CbPositionControlFreeSpace::max_angular_velocity = 1.0

private

Definition at line 45 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ max_angular_yaw_speed_

double cl_nitrosz::CbPositionControlFreeSpace::max_angular_yaw_speed_

private

Definition at line 35 of file cb_position_control_free_space.hpp.

◆ max_linear_velocity

double cl_nitrosz::CbPositionControlFreeSpace::max_linear_velocity = 1.0

private

Definition at line 44 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ prev_error_angular_

double cl_nitrosz::CbPositionControlFreeSpace::prev_error_angular_ = 0.0

private

Definition at line 38 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ prev_error_linear_

double cl_nitrosz::CbPositionControlFreeSpace::prev_error_linear_ = 0.0

private

Definition at line 37 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ target_pose_

geometry_msgs::msg::Pose cl_nitrosz::CbPositionControlFreeSpace::target_pose_

Definition at line 54 of file cb_position_control_free_space.hpp.

Referenced by cl_nitrosz::CbNavigateNextWaypointFree::onEntry(), and onEntry().

◆ targetYaw_

double cl_nitrosz::CbPositionControlFreeSpace::targetYaw_

private

Definition at line 32 of file cb_position_control_free_space.hpp.

◆ threshold_distance_

double cl_nitrosz::CbPositionControlFreeSpace::threshold_distance_ = 3.0

Definition at line 52 of file cb_position_control_free_space.hpp.

Referenced by onEntry().

◆ yaw_goal_tolerance_rads_

double cl_nitrosz::CbPositionControlFreeSpace::yaw_goal_tolerance_rads_ = 0.1

Definition at line 50 of file cb_position_control_free_space.hpp.

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

smacc2_client_library/nitrosz_client/nitrosz_client/include/nitrosz_client/client_behaviors/cb_position_control_free_space.hpp
smacc2_client_library/nitrosz_client/nitrosz_client/src/nitrosz_client/client_behaviors/cb_position_control_free_space.cpp

Public Member Functions

Public Attributes

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CbPositionControlFreeSpace()

Member Function Documentation

◆ onEntry()

◆ onExit()

◆ updateParameters()

Member Data Documentation

◆ cmd_vel_pub_

◆ goalReached_

◆ integral_angular_

◆ integral_linear_

◆ k_betta_

◆ max_angular_velocity

◆ max_angular_yaw_speed_

◆ max_linear_velocity

◆ prev_error_angular_

◆ prev_error_linear_

◆ target_pose_

◆ targetYaw_

◆ threshold_distance_

◆ yaw_goal_tolerance_rads_