cl_moveit2z::CbMoveCartesianRelative2 Class Reference

#include <cb_move_cartesian_relative2.hpp>

Inheritance diagram for cl_moveit2z::CbMoveCartesianRelative2:

Collaboration diagram for cl_moveit2z::CbMoveCartesianRelative2:

Public Member Functions
	CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink)
	CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset)
virtual	~CbMoveCartesianRelative2 ()
void	generateTrajectory () override
	CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink)
	CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset)
virtual	~CbMoveCartesianRelative2 ()
void	generateTrajectory () 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	onEntry () override
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	onEntry () override
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
geometry_msgs::msg::Vector3	offset_
std::optional< double >	linearSpeed_m_s_
Public Attributes inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
std::optional< std::string >	group_
std::optional< std::string >	tipLink_
std::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Private Attributes
std::string	globalFrame_

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)
virtual void	createMarkers ()
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)
virtual void	createMarkers ()
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::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_
ClMoveit2z *	movegroupClient_ = nullptr
visualization_msgs::msg::MarkerArray	beahiorMarkers_

Detailed Description

Definition at line 31 of file cb_move_cartesian_relative2.hpp.

Constructor & Destructor Documentation

CbMoveCartesianRelative2() [1/4]

cl_moveit2z::CbMoveCartesianRelative2::CbMoveCartesianRelative2 ( std::string referenceFrame, std::string tipLink )

inline

Definition at line 41 of file cb_move_cartesian_relative2.hpp.

  : CbMoveEndEffectorTrajectory(tipLink)
  {
    globalFrame_ = referenceFrame;
  }

References globalFrame_.

CbMoveCartesianRelative2() [2/4]

cl_moveit2z::CbMoveCartesianRelative2::CbMoveCartesianRelative2 ( std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset )

inline

Definition at line 47 of file cb_move_cartesian_relative2.hpp.

  : CbMoveEndEffectorTrajectory(tipLink)
  {
    globalFrame_ = referenceFrame;
    offset_ = offset;
  }

References globalFrame_, and offset_.

~CbMoveCartesianRelative2() [1/2]

cl_moveit2z::CbMoveCartesianRelative2::~CbMoveCartesianRelative2 ( )

inlinevirtual

Definition at line 55 of file cb_move_cartesian_relative2.hpp.

{}

CbMoveCartesianRelative2() [3/4]

cl_moveit2z::CbMoveCartesianRelative2::CbMoveCartesianRelative2	(	std::string	referenceFrame,
		std::string	tipLink )

CbMoveCartesianRelative2() [4/4]

cl_moveit2z::CbMoveCartesianRelative2::CbMoveCartesianRelative2	(	std::string	referenceFrame,
		std::string	tipLink,
		geometry_msgs::msg::Vector3	offset )

~CbMoveCartesianRelative2() [2/2]

virtual cl_moveit2z::CbMoveCartesianRelative2::~CbMoveCartesianRelative2 ( )

virtual

Member Function Documentation

generateTrajectory() [1/2]

void cl_moveit2z::CbMoveCartesianRelative2::generateTrajectory ( )

inlineoverridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 57 of file cb_move_cartesian_relative2.hpp.

  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] generating trajectory");
    // at least 1 sample per centimeter (average)
    const double METERS_PER_SAMPLE = 0.001;
 
    float dist_meters = 0;
    tf2::Vector3 voffset;
    tf2::fromMsg(offset_, voffset);
 
    float totallineardist = voffset.length();
 
    int totalSamplesCount = totallineardist / METERS_PER_SAMPLE;
    int steps = totallineardist / METERS_PER_SAMPLE;
 
    float interpolation_factor_step = 1.0 / totalSamplesCount;
 
    double secondsPerSample;
 
    if (!linearSpeed_m_s_)
    {
      linearSpeed_m_s_ = 0.1;
    }
 
    secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
 
    tf2::Stamped<tf2::Transform> currentEndEffectorTransform;
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current end effector pose");
    this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
 
    tf2::Transform finalEndEffectorTransform = currentEndEffectorTransform;
    finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset);
 
    float linc = totallineardist / steps;  // METERS_PER_SAMPLE with sign
    float interpolation_factor = 0;
 
    rclcpp::Time startTime = getNode()->now();
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps);
 
    for (float i = 0; i < steps; i++)
    {
      interpolation_factor += interpolation_factor_step;
      dist_meters += linc;
 
      tf2::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp(
        finalEndEffectorTransform.getOrigin(), interpolation_factor);
 
      tf2::Transform pose;
      pose.setOrigin(vi);
      pose.setRotation(currentEndEffectorTransform.getRotation());
 
      geometry_msgs::msg::PoseStamped pointerPose;
      tf2::toMsg(pose, pointerPose.pose);
      pointerPose.header.frame_id = globalFrame_;
      pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample);
 
      this->endEffectorTrajectory_.push_back(pointerPose);
    }
 
    RCLCPP_INFO_STREAM(
      getLogger(),
      "[" << getName() << "] Target End efector Pose: " << this->endEffectorTrajectory_.back());
  }

References cl_moveit2z::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_moveit2z::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), globalFrame_, linearSpeed_m_s_, and offset_.

Here is the call graph for this function:

generateTrajectory() [2/2]

void cl_moveit2z::CbMoveCartesianRelative2::generateTrajectory ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Member Data Documentation

globalFrame_

std::string cl_moveit2z::CbMoveCartesianRelative2::globalFrame_

private

Definition at line 122 of file cb_move_cartesian_relative2.hpp.

Referenced by CbMoveCartesianRelative2(), CbMoveCartesianRelative2(), and generateTrajectory().

linearSpeed_m_s_

std::optional< double > cl_moveit2z::CbMoveCartesianRelative2::linearSpeed_m_s_

Definition at line 39 of file cb_move_cartesian_relative2.hpp.

Referenced by generateTrajectory().

offset_

geometry_msgs::msg::Vector3 cl_moveit2z::CbMoveCartesianRelative2::offset_

Definition at line 37 of file cb_move_cartesian_relative2.hpp.

Referenced by CbMoveCartesianRelative2(), and generateTrajectory().

---

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

• smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.hpp
• smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_move_cartesian_relative2.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_move_cartesian_relative2.cpp