#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

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

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	onStateOrthogonalAllocation ()

virtual void	onExit () override

Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

virtual	~SmaccAsyncClientBehavior ()

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFailure (TCallback callback, T *object)

void	requestForceFinish ()

void	executeOnEntry () override

void	executeOnExit () override

void	waitOnEntryThread (bool requestFinish)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	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, ComponentRequirement requirementType=ComponentRequirement::SOFT)

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)

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

CpMoveGroupInterface *	cpMoveGroup_ = nullptr

visualization_msgs::msg::MarkerArray	beahiorMarkers_

Detailed Description

Definition at line 34 of file cb_end_effector_rotate.hpp.

Constructor & Destructor Documentation

◆ CbEndEffectorRotate()

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()

virtual cl_moveit2z::CbEndEffectorRotate::~CbEndEffectorRotate ( )

inlinevirtual

Definition at line 43 of file cb_end_effector_rotate.hpp.

43{}

Member Function Documentation

◆ onEntry()

virtual 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, smacc2::ComponentRequirement::SOFT);  // Optional component
 
    tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
 
    int attempts = 3;
 
    this->requiresComponent(cpMoveGroup_);
    if (!tipLink_)
    {
      tipLink_ = this->cpMoveGroup_->moveGroupClientInterface->getEndEffectorLink();
      RCLCPP_WARN_STREAM(
        getLogger(),
        "[" << getName() << "] tip unspecified, using default end effector: " << *tipLink_);
    }
 
    while (attempts > 0)
    {
      try
      {
        auto pivotFrameName = this->cpMoveGroup_->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();
  }