#include <cb_move_cartesian_relative2.h>

Inheritance diagram for cl_move_group_interface::CbMoveCartesianRelative2:

Collaboration diagram for cl_move_group_interface::CbMoveCartesianRelative2:

Public Member Functions
	CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink)

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

virtual void	generateTrajectory () override

Public Member Functions inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
	CbMoveEndEffectorTrajectory (std::string tipLink="")

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

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

virtual void	onEntry () override

virtual void	onExit () override

virtual void	update () override

Public Member Functions inherited from smacc::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)

Public Member Functions inherited from smacc::ISmaccClientBehavior
	ISmaccClientBehavior ()

virtual	~ISmaccClientBehavior ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage)

ros::NodeHandle	getNode ()

Public Member Functions inherited from smacc::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (ros::Duration duration)

void	executeUpdate ()

void	setUpdatePeriod (ros::Duration duration)

Public Attributes
geometry_msgs::Vector3	offset_

boost::optional< double >	linearSpeed_m_s_

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

boost::optional< std::string >	tipLink_

boost::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Private Attributes
std::string	globalFrame_

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

void	executeJointSpaceTrajectory (const moveit_msgs::RobotTrajectory &computedJointTrajectory)

virtual void	generateTrajectory ()

virtual void	createMarkers ()

void	getCurrentEndEffectorPose (std::string globalFrame, tf::StampedTransform &currentEndEffectorTransform)

Protected Member Functions inherited from smacc::SmaccAsyncClientBehavior
virtual void	executeOnEntry () override

virtual void	executeOnExit () override

void	postSuccessEvent ()

void	postFailureEvent ()

virtual void	dispose () override

Protected Member Functions inherited from smacc::ISmaccClientBehavior
virtual void	runtimeConfigure ()

virtual void	onEntry ()

virtual void	onExit ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

ISmaccState *	getCurrentState ()

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

virtual void	dispose ()

virtual void	update ()=0

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

ClMoveGroup *	movegroupClient_ = nullptr

visualization_msgs::MarkerArray	beahiorMarkers_

Detailed Description

Definition at line 15 of file cb_move_cartesian_relative2.h.

Constructor & Destructor Documentation

◆ CbMoveCartesianRelative2() [1/2]

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

inline

Definition at line 22 of file cb_move_cartesian_relative2.h.

            : CbMoveEndEffectorTrajectory(tipLink)
        {
            globalFrame_ = referenceFrame;
        }

References globalFrame_.

◆ CbMoveCartesianRelative2() [2/2]

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

inline

Definition at line 28 of file cb_move_cartesian_relative2.h.

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

References globalFrame_, and offset_.

Member Function Documentation

◆ generateTrajectory()

virtual void cl_move_group_interface::CbMoveCartesianRelative2::generateTrajectory ( )

inlineoverridevirtual

Reimplemented from cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 35 of file cb_move_cartesian_relative2.h.

        {
            // at least 1 sample per centimeter (average)
            const double METERS_PER_SAMPLE = 0.001;
 
            float dist_meters = 0;
            tf::Vector3 voffset;
            tf::vector3MsgToTF(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_);
 
            tf::StampedTransform currentEndEffectorTransform;
 
            this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
 
            tf::Transform finalEndEffectorTransform = currentEndEffectorTransform;
            finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset);
 
            float linc = totallineardist / steps; // METERS_PER_SAMPLE with sign
            float interpolation_factor = 0;
 
            ros::Time startTime = ros::Time::now();
 
            for (float i = 0; i < steps; i++)
            {
                interpolation_factor += interpolation_factor_step;
                dist_meters += linc;
 
                tf::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp(finalEndEffectorTransform.getOrigin(), interpolation_factor);
 
                tf::Transform pose;
                pose.setOrigin(vi);
                pose.setRotation(currentEndEffectorTransform.getRotation());
 
                geometry_msgs::PoseStamped pointerPose;
                tf::poseTFToMsg(pose, pointerPose.pose);
                pointerPose.header.frame_id = globalFrame_;
                pointerPose.header.stamp = startTime + ros::Duration(i * secondsPerSample);
 
                this->endEffectorTrajectory_.push_back(pointerPose);
            }
 
            ROS_INFO_STREAM("[CbMoveEndEffectorRelative2] Target End efector Pose: " << this->endEffectorTrajectory_.back());
 
        }