SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | List of all members
cl_move_group_interface::CbMoveEndEffectorRelative Class Reference

#include <cb_move_end_effector_relative.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveEndEffectorRelative:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveEndEffectorRelative:
Collaboration graph

Public Member Functions

 CbMoveEndEffectorRelative ()
 
 CbMoveEndEffectorRelative (geometry_msgs::msg::Transform transform)
 
virtual void onEntry () override
 
virtual void onExit () 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)
 
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 ()
 
ISmaccStateMachinegetStateMachine ()
 
std::string getName () const
 
template<typename SmaccClientType >
void requiresClient (SmaccClientType *&storage)
 
template<typename SmaccComponentType >
void requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)
 
virtual void onEntry ()
 
virtual void onExit ()
 
virtual void executeOnEntry ()
 
virtual void executeOnExit ()
 

Public Attributes

geometry_msgs::msg::Transform transform_
 
std::optional< std::string > group_
 

Protected Member Functions

void moveRelative (moveit::planning_interface::MoveGroupInterface &moveGroupinterface, geometry_msgs::msg::Transform &transformOffset)
 
- 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 ()
 
template<typename EventType >
void postEvent (const EventType &ev)
 
template<typename EventType >
void postEvent ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
virtual rclcpp::Node::SharedPtr getNode () const
 
virtual rclcpp::Logger getLogger () const
 

Protected Attributes

ClMoveGroupmovegroupClient_
 

Detailed Description

Definition at line 28 of file cb_move_end_effector_relative.hpp.

Constructor & Destructor Documentation

◆ CbMoveEndEffectorRelative() [1/2]

cl_move_group_interface::CbMoveEndEffectorRelative::CbMoveEndEffectorRelative ( )

Definition at line 34 of file cb_move_end_effector_relative.cpp.

34{ transform_.rotation.w = 1; }

References transform_.

◆ CbMoveEndEffectorRelative() [2/2]

cl_move_group_interface::CbMoveEndEffectorRelative::CbMoveEndEffectorRelative ( geometry_msgs::msg::Transform  transform)

Definition at line 36 of file cb_move_end_effector_relative.cpp.

37: transform_(transform)
38{
39}

Member Function Documentation

◆ moveRelative()

void cl_move_group_interface::CbMoveEndEffectorRelative::moveRelative ( moveit::planning_interface::MoveGroupInterface &  moveGroupinterface,
geometry_msgs::msg::Transform &  transformOffset 
)
protected

Definition at line 63 of file cb_move_end_effector_relative.cpp.

66{
67 auto referenceStartPose = moveGroupInterface.getCurrentPose();
68 tf2::Quaternion currentOrientation;
69 tf2::convert(referenceStartPose.pose.orientation, currentOrientation);
70 tf2::Quaternion desiredRelativeRotation;
71
72 tf2::convert(transformOffset.rotation, desiredRelativeRotation);
73
74 auto targetOrientation = desiredRelativeRotation * currentOrientation;
75
76 geometry_msgs::msg::PoseStamped targetObjectPose = referenceStartPose;
77
78 targetObjectPose.pose.orientation = tf2::toMsg(targetOrientation);
79 targetObjectPose.pose.position.x += transformOffset.translation.x;
80 targetObjectPose.pose.position.y += transformOffset.translation.y;
81 targetObjectPose.pose.position.z += transformOffset.translation.z;
82
83 moveGroupInterface.setPlanningTime(1.0);
84
85 RCLCPP_INFO_STREAM(
86 getLogger(), "[CbMoveEndEffectorRelative] Target End efector Pose: " << targetObjectPose);
87
88 moveGroupInterface.setPoseTarget(targetObjectPose);
89 moveGroupInterface.setPoseReferenceFrame("map");
90
91 moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
92 bool success =
93 (moveGroupInterface.plan(computedMotionPlan) ==
94 moveit::planning_interface::MoveItErrorCode::SUCCESS);
95 RCLCPP_INFO(getLogger(), "Success Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
96
97 if (success)
98 {
99 auto executionResult = moveGroupInterface.execute(computedMotionPlan);
100
101 if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
102 {
103 RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution succeeded");
105 this->postSuccessEvent();
106 }
107 else
108 {
109 moveGroupInterface.setPoseTarget(
110 referenceStartPose); // undo to avoid abort-repeat state issues with this relative motion
111 RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution failed");
113 this->postFailureEvent();
114 }
115 }
116 else
117 {
118 moveGroupInterface.setPoseTarget(
119 referenceStartPose); //undo since we did not executed the motion
120 RCLCPP_INFO(getLogger(), "[CbMoveEndEffectorRelative] motion execution failed");
122 this->postFailureEvent();
123 }
124}
virtual rclcpp::Logger getLogger() const

References smacc2::ISmaccClientBehavior::getLogger(), movegroupClient_, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), and smacc2::SmaccAsyncClientBehavior::postSuccessEvent().

Referenced by onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onEntry()

void cl_move_group_interface::CbMoveEndEffectorRelative::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 41 of file cb_move_end_effector_relative.cpp.

42{
43 RCLCPP_INFO_STREAM(
44 getLogger(),
45 "[CbMoveEndEffectorRelative] Transform end effector pose relative: " << transform_);
46
48
49 if (this->group_)
50 {
51 moveit::planning_interface::MoveGroupInterface move_group(
52 getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
53 this->moveRelative(move_group, this->transform_);
54 }
55 else
56 {
58 }
59}
void moveRelative(moveit::planning_interface::MoveGroupInterface &moveGroupinterface, geometry_msgs::msg::Transform &transformOffset)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode() const
void requiresClient(SmaccClientType *&storage)

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getNode(), group_, movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, moveRelative(), smacc2::ISmaccClientBehavior::requiresClient(), and transform_.

Here is the call graph for this function:

◆ onExit()

void cl_move_group_interface::CbMoveEndEffectorRelative::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 61 of file cb_move_end_effector_relative.cpp.

61{}

Member Data Documentation

◆ group_

std::optional<std::string> cl_move_group_interface::CbMoveEndEffectorRelative::group_

Definition at line 33 of file cb_move_end_effector_relative.hpp.

Referenced by onEntry().

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveEndEffectorRelative::movegroupClient_
protected

Definition at line 48 of file cb_move_end_effector_relative.hpp.

Referenced by moveRelative(), and onEntry().

◆ transform_

geometry_msgs::msg::Transform cl_move_group_interface::CbMoveEndEffectorRelative::transform_

Definition at line 31 of file cb_move_end_effector_relative.hpp.

Referenced by CbMoveEndEffectorRelative(), and onEntry().


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