cl_moveit2z::CbMoveCartesianRelative Class Reference

#include <cb_move_cartesian_relative.hpp>

Inheritance diagram for cl_moveit2z::CbMoveCartesianRelative:

Collaboration diagram for cl_moveit2z::CbMoveCartesianRelative:

Public Member Functions
	CbMoveCartesianRelative ()
	CbMoveCartesianRelative (geometry_msgs::msg::Vector3 offset)
virtual void	onEntry () override
virtual void	onExit () override
void	moveRelativeCartesian (moveit::planning_interface::MoveGroupInterface *movegroupClient, geometry_msgs::msg::Vector3 &offset)
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 Attributes
geometry_msgs::msg::Vector3	offset_
std::optional< double >	scalingFactor_
std::optional< std::string >	group_
ClMoveit2z *	moveGroupSmaccClient_

Additional Inherited Members
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

Detailed Description

Definition at line 30 of file cb_move_cartesian_relative.hpp.

Constructor & Destructor Documentation

CbMoveCartesianRelative() [1/2]

cl_moveit2z::CbMoveCartesianRelative::CbMoveCartesianRelative

(

)

Definition at line 27 of file cb_move_cartesian_relative.cpp.

{}

CbMoveCartesianRelative() [2/2]

cl_moveit2z::CbMoveCartesianRelative::CbMoveCartesianRelative

(

geometry_msgs::msg::Vector3

offset

)

Definition at line 29 of file cb_move_cartesian_relative.cpp.

: offset_(offset)
{
}

Member Function Documentation

moveRelativeCartesian()

void cl_moveit2z::CbMoveCartesianRelative::moveRelativeCartesian	(	moveit::planning_interface::MoveGroupInterface *	movegroupClient,
		geometry_msgs::msg::Vector3 &	offset )

Definition at line 53 of file cb_move_cartesian_relative.cpp.

{
  std::vector<geometry_msgs::msg::Pose> waypoints;
 
  // this one was working fine but the issue is that for relative motions it grows up on ABORT-State-Loop pattern.
  // But, we need current pose because maybe setCurrentPose was not set by previous behaviors. The only solution would be
  // distinguishing between the execution error and the planning error with no state change
  //auto referenceStartPose = movegroupClient->getPoseTarget();
  auto referenceStartPose = movegroupClient->getCurrentPose();
  movegroupClient->setPoseReferenceFrame(referenceStartPose.header.frame_id);
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[CbMoveCartesianRelative] RELATIVE MOTION, SOURCE POSE: " << referenceStartPose);
  RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Offset: " << offset);
 
  waypoints.push_back(referenceStartPose.pose);  // up and out
 
  auto endPose = referenceStartPose.pose;
 
  endPose.position.x += offset.x;
  endPose.position.y += offset.y;
  endPose.position.z += offset.z;
 
  RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] DESTINY POSE: " << endPose);
 
  // target_pose2.position.x -= 0.15;
  waypoints.push_back(endPose);  // left
 
  movegroupClient->setPoseTarget(endPose);
 
  float scalinf = 0.5;
  if (scalingFactor_) scalinf = *scalingFactor_;
 
  RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Using scaling factor: " << scalinf);
  movegroupClient->setMaxVelocityScalingFactor(scalinf);
 
  moveit_msgs::msg::RobotTrajectory trajectory;
  double fraction = movegroupClient->computeCartesianPath(
    waypoints,
    0.01,  // eef_step
    0.00,  // jump_threshold
    trajectory);
 
  moveit::core::MoveItErrorCode behaviorResult;
  if (fraction != 1.0 || fraction == -1)
  {
    RCLCPP_WARN_STREAM(
      getLogger(),
      "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje. Execution skipped "
      "because not 100% of cartesian motion: "
        << fraction * 100 << "%");
    behaviorResult = moveit::core::MoveItErrorCode::PLANNING_FAILED;
  }
  else
  {
    RCLCPP_INFO_STREAM(
      getLogger(),
      "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje: " << fraction);
 
    moveit::planning_interface::MoveGroupInterface::Plan grasp_pose_plan;
 
    // grasp_pose_plan.start_state_ = *(moveGroupInterface.getCurrentState());
    grasp_pose_plan.trajectory_ = trajectory;
    behaviorResult = movegroupClient->execute(grasp_pose_plan);
  }
 
  if (behaviorResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    RCLCPP_INFO(
      getLogger(), "[CbMoveCartesianRelative] relative motion execution succeeded: fraction %lf.",
      fraction);
    moveGroupSmaccClient_->postEventMotionExecutionSucceded();
    this->postSuccessEvent();
  }
  else
  {
    movegroupClient->setPoseTarget(
      referenceStartPose);  // undo changes since we did not executed the motion
    RCLCPP_INFO(
      getLogger(), "[CbMoveCartesianRelative] relative motion execution failed: fraction %lf.",
      fraction);
    moveGroupSmaccClient_->postEventMotionExecutionFailed();
    this->postFailureEvent();
  }
}

References smacc2::ISmaccClientBehavior::getLogger(), moveGroupSmaccClient_, cl_moveit2z::ClMoveit2z::postEventMotionExecutionFailed(), cl_moveit2z::ClMoveit2z::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), and scalingFactor_.

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

onEntry()

void cl_moveit2z::CbMoveCartesianRelative::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 34 of file cb_move_cartesian_relative.cpp.

{
  this->requiresClient(moveGroupSmaccClient_);
 
  if (this->group_)
  {
    moveit::planning_interface::MoveGroupInterface move_group(
      getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
    this->moveRelativeCartesian(&move_group, offset_);
  }
  else
  {
    this->moveRelativeCartesian(moveGroupSmaccClient_->moveGroupClientInterface.get(), offset_);
  }
}

References smacc2::ISmaccClientBehavior::getNode(), group_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, moveGroupSmaccClient_, moveRelativeCartesian(), offset_, and smacc2::ISmaccClientBehavior::requiresClient().

Here is the call graph for this function:

onExit()

void cl_moveit2z::CbMoveCartesianRelative::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 50 of file cb_move_cartesian_relative.cpp.

{}

Member Data Documentation

group_

std::optional<std::string> cl_moveit2z::CbMoveCartesianRelative::group_

Definition at line 37 of file cb_move_cartesian_relative.hpp.

Referenced by onEntry().

moveGroupSmaccClient_

ClMoveit2z* cl_moveit2z::CbMoveCartesianRelative::moveGroupSmaccClient_

Definition at line 52 of file cb_move_cartesian_relative.hpp.

Referenced by moveRelativeCartesian(), and onEntry().

offset_

geometry_msgs::msg::Vector3 cl_moveit2z::CbMoveCartesianRelative::offset_

Definition at line 33 of file cb_move_cartesian_relative.hpp.

Referenced by onEntry().

scalingFactor_

std::optional<double> cl_moveit2z::CbMoveCartesianRelative::scalingFactor_

Definition at line 35 of file cb_move_cartesian_relative.hpp.

Referenced by moveRelativeCartesian().

---

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

• smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_move_cartesian_relative.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_move_cartesian_relative.cpp