#include <cb_move_cartesian_relative2.hpp>

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::msg::Vector3 offset)

virtual	~CbMoveCartesianRelative2 ()

void	generateTrajectory () override

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

virtual void	update () 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)

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)

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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)

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

virtual void	generateTrajectory ()=0

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

Protected Member Functions inherited from smacc2::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	dispose ()

virtual rclcpp::Node::SharedPtr	getNode ()

virtual rclcpp::Logger	getLogger ()

virtual void	update ()=0

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

ClMoveGroup *	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/2]

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

Definition at line 28 of file cb_move_cartesian_relative2.cpp.

: CbMoveEndEffectorTrajectory(tipLink)
{
  globalFrame_ = referenceFrame;
}

References globalFrame_.

◆ CbMoveCartesianRelative2() [2/2]

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

Definition at line 34 of file cb_move_cartesian_relative2.cpp.

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

References globalFrame_, and offset_.

◆ ~CbMoveCartesianRelative2()

cl_move_group_interface::CbMoveCartesianRelative2::~CbMoveCartesianRelative2 ( )

virtual

Definition at line 42 of file cb_move_cartesian_relative2.cpp.

42{}

Member Function Documentation

◆ generateTrajectory()

void cl_move_group_interface::CbMoveCartesianRelative2::generateTrajectory ( )

overridevirtual

Implements cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 44 of file cb_move_cartesian_relative2.cpp.

{
  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_move_group_interface::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_move_group_interface::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:

Member Data Documentation

◆ globalFrame_

std::string cl_move_group_interface::CbMoveCartesianRelative2::globalFrame_

private

Definition at line 48 of file cb_move_cartesian_relative2.hpp.

Referenced by CbMoveCartesianRelative2(), and generateTrajectory().

◆ linearSpeed_m_s_

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

Definition at line 36 of file cb_move_cartesian_relative2.hpp.

Referenced by generateTrajectory().

◆ offset_

geometry_msgs::msg::Vector3 cl_move_group_interface::CbMoveCartesianRelative2::offset_

Definition at line 34 of file cb_move_cartesian_relative2.hpp.