#include <cb_end_effector_rotate.hpp>

Inheritance diagram for cl_moveit2z::CbEndEffectorRotate:

Collaboration diagram for cl_moveit2z::CbEndEffectorRotate:

Public Member Functions
	CbEndEffectorRotate (double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

virtual	~CbEndEffectorRotate ()

virtual void	onEntry () override

	CbEndEffectorRotate (double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

virtual	~CbEndEffectorRotate ()

virtual void	onEntry () override

Public Member Functions inherited from cl_moveit2z::CbCircularPivotMotion
	CbCircularPivotMotion (std::optional< std::string > tipLink=std::nullopt)

	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, const geometry_msgs::msg::Pose &relativeInitialPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

virtual void	generateTrajectory () override

virtual void	createMarkers () override

	CbCircularPivotMotion (std::optional< std::string > tipLink=std::nullopt)

	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, const geometry_msgs::msg::Pose &relativeInitialPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)

virtual void	generateTrajectory () override

virtual void	createMarkers () override

Public Member Functions inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)

	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

virtual void	onExit () override

	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)

	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

virtual void	onExit () override

virtual void	update () 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 Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (rclcpp::Duration duration)

void	executeUpdate (rclcpp::Node::SharedPtr node)

void	setUpdatePeriod (rclcpp::Duration duration)

Public Attributes
std::optional< std::string >	tipLink

Public Attributes inherited from cl_moveit2z::CbCircularPivotMotion
std::optional< double >	angularSpeed_rad_s_

std::optional< double >	linearSpeed_m_s_

std::optional< geometry_msgs::msg::Pose >	relativeInitialPose_

Public Attributes inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
std::optional< std::string >	group_

std::optional< std::string >	tipLink_

std::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Additional Inherited Members
Protected Member Functions inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)

void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)

void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)

ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)

void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)

void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)

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

Protected Member Functions inherited from smacc2::ISmaccUpdatable
Protected Attributes inherited from cl_moveit2z::CbCircularPivotMotion
geometry_msgs::msg::PoseStamped	planePivotPose_

double	deltaRadians_

Protected Attributes inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_

ClMoveit2z *	movegroupClient_ = nullptr

visualization_msgs::msg::MarkerArray	beahiorMarkers_

Detailed Description

Definition at line 26 of file cb_end_effector_rotate.hpp.

Constructor & Destructor Documentation

◆ CbEndEffectorRotate() [1/2]

cl_moveit2z::CbEndEffectorRotate::CbEndEffectorRotate	(	double	deltaRadians,
		std::optional< std::string >	tipLink = std::nullopt )

inline

Definition at line 37 of file cb_end_effector_rotate.hpp.

  : CbCircularPivotMotion(tipLink)
  {
    deltaRadians_ = deltaRadians;
  }

References cl_moveit2z::CbCircularPivotMotion::deltaRadians_.

◆ ~CbEndEffectorRotate() [1/2]

cl_moveit2z::CbEndEffectorRotate::~CbEndEffectorRotate ( )

inlinevirtual

Definition at line 43 of file cb_end_effector_rotate.hpp.

43{}

◆ CbEndEffectorRotate() [2/2]

cl_moveit2z::CbEndEffectorRotate::CbEndEffectorRotate	(	double	deltaRadians,
		std::optional< std::string >	tipLink = std::nullopt )

◆ ~CbEndEffectorRotate() [2/2]

virtual cl_moveit2z::CbEndEffectorRotate::~CbEndEffectorRotate ( )

virtual

Member Function Documentation

◆ onEntry() [1/2]

void cl_moveit2z::CbEndEffectorRotate::onEntry ( )

inlineoverridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 45 of file cb_end_effector_rotate.hpp.

  {
    // Use CpTfListener component for transform lookups
    CpTfListener * tfListener = nullptr;
    this->requiresComponent(tfListener, false);  // Optional component
 
    tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
 
    int attempts = 3;
 
    this->requiresClient(movegroupClient_);
    if (!tipLink_)
    {
      tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
      RCLCPP_WARN_STREAM(
        getLogger(),
        "[" << getName() << "] tip unspecified, using default end effector: " << *tipLink_);
    }
 
    while (attempts > 0)
    {
      try
      {
        auto pivotFrameName =
          this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
 
        if (tfListener != nullptr)
        {
          // Use component-based TF listener (preferred)
          auto transformOpt =
            tfListener->lookupTransform(pivotFrameName, *tipLink_, rclcpp::Time());
 
          if (transformOpt)
          {
            tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame);
            tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose);
            this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_;
            this->planePivotPose_.header.stamp =
              rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count());
            break;
          }
          else
          {
            RCLCPP_ERROR_STREAM(
              getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
                               << " to " << pivotFrameName << ". Attempt countdown: " << attempts);
            rclcpp::sleep_for(500ms);
            attempts--;
          }
        }
        else
        {
          // Fallback to legacy TF2 usage if component not available
          RCLCPP_WARN_STREAM(
            getLogger(),
            "[" << getName()
                << "] CpTfListener component not available, using legacy TF2 (consider "
                   "adding CpTfListener component)");
          tf2_ros::Buffer tfBuffer(getNode()->get_clock());
          tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
 
          tf2::fromMsg(
            tfBuffer.lookupTransform(
              pivotFrameName, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
            endEffectorInPivotFrame);
 
          tf2::toMsg(endEffectorInPivotFrame, this->planePivotPose_.pose);
          this->planePivotPose_.header.frame_id = endEffectorInPivotFrame.frame_id_;
          this->planePivotPose_.header.stamp =
            rclcpp::Time(endEffectorInPivotFrame.stamp_.time_since_epoch().count());
          break;
        }
      }
      catch (const std::exception & e)
      {
        RCLCPP_ERROR_STREAM(getLogger(), e.what() << ". Attempt countdown: " << attempts);
        rclcpp::sleep_for(500ms);
        attempts--;
      }
    }
 
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] pivotPose: " << planePivotPose_);
 
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] calling base CbCircularPivotMotion::onEntry");
    CbCircularPivotMotion::onEntry();
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_moveit2z::CpTfListener::lookupTransform(), cl_moveit2z::CbMoveEndEffectorTrajectory::movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, smacc2::ISmaccClientBehavior::onEntry(), cl_moveit2z::CbCircularPivotMotion::planePivotPose_, smacc2::ISmaccClientBehavior::requiresClient(), smacc2::ISmaccClientBehavior::requiresComponent(), and cl_moveit2z::CbMoveEndEffectorTrajectory::tipLink_.

Here is the call graph for this function:

◆ onEntry() [2/2]

virtual void cl_moveit2z::CbEndEffectorRotate::onEntry ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Member Data Documentation

◆ tipLink

std::optional< std::string > cl_moveit2z::CbEndEffectorRotate::tipLink

Definition at line 133 of file cb_end_effector_rotate.hpp.

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

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_end_effector_rotate.hpp
smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_end_effector_rotate.hpp
smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_end_effector_rotate.cpp

Public Member Functions

Public Attributes

Additional Inherited Members