SMACC2
Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | List of all members
cl_move_group_interface::CbMoveEndEffector Class Reference

#include <cb_move_end_effector.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveEndEffector:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveEndEffector:
Collaboration graph

Public Member Functions

 CbMoveEndEffector ()
 
 CbMoveEndEffector (geometry_msgs::msg::PoseStamped target_pose, std::string tip_link="")
 
virtual void onEntry () 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 ()
 
ISmaccStateMachinegetStateMachine ()
 
std::string getName () const
 
template<typename SmaccClientType >
void requiresClient (SmaccClientType *&storage)
 
template<typename SmaccComponentType >
void requiresComponent (SmaccComponentType *&storage)
 

Public Attributes

geometry_msgs::msg::PoseStamped targetPose
 
std::string tip_link_
 
std::optional< std::string > group_
 

Protected Member Functions

bool moveToAbsolutePose (moveit::planning_interface::MoveGroupInterface &moveGroupInterface, geometry_msgs::msg::PoseStamped &targetObjectPose)
 
- 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 ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
virtual rclcpp::Node::SharedPtr getNode ()
 
virtual rclcpp::Logger getLogger ()
 

Protected Attributes

ClMoveGroupmovegroupClient_
 

Detailed Description

Definition at line 28 of file cb_move_end_effector.hpp.

Constructor & Destructor Documentation

◆ CbMoveEndEffector() [1/2]

cl_move_group_interface::CbMoveEndEffector::CbMoveEndEffector ( )

Definition at line 33 of file cb_move_end_effector.cpp.

33{}

◆ CbMoveEndEffector() [2/2]

cl_move_group_interface::CbMoveEndEffector::CbMoveEndEffector ( geometry_msgs::msg::PoseStamped  target_pose,
std::string  tip_link = "" 
)

Definition at line 35 of file cb_move_end_effector.cpp.

37: targetPose(target_pose)
38{
39 tip_link_ = tip_link;
40}
geometry_msgs::msg::PoseStamped targetPose

References tip_link_.

Member Function Documentation

◆ moveToAbsolutePose()

bool cl_move_group_interface::CbMoveEndEffector::moveToAbsolutePose ( moveit::planning_interface::MoveGroupInterface &  moveGroupInterface,
geometry_msgs::msg::PoseStamped &  targetObjectPose 
)
protected

Definition at line 63 of file cb_move_end_effector.cpp.

66{
67 // auto & planningSceneInterface = movegroupClient_->planningSceneInterface;
68 RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds");
69 rclcpp::sleep_for(500ms);
70
71 moveGroupInterface.setPlanningTime(1.0);
72
73 RCLCPP_INFO_STREAM(
74 getLogger(), "[CbMoveEndEffector] Target End efector Pose: " << targetObjectPose);
75
76 moveGroupInterface.setPoseTarget(targetObjectPose, tip_link_);
77 moveGroupInterface.setPoseReferenceFrame(targetObjectPose.header.frame_id);
78
79 moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
80 bool success =
81 (moveGroupInterface.plan(computedMotionPlan) ==
82 moveit::planning_interface::MoveItErrorCode::SUCCESS);
83 RCLCPP_INFO(
84 getLogger(), "[CbMoveEndEffector] Success Visualizing plan 1 (pose goal) %s",
85 success ? "" : "FAILED");
86
87 if (success)
88 {
89 auto executionResult = moveGroupInterface.execute(computedMotionPlan);
90
91 if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
92 {
93 RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution succeeded");
95 this->postSuccessEvent();
96 }
97 else
98 {
99 RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed");
101 this->postFailureEvent();
102 }
103 }
104 else
105 {
106 RCLCPP_INFO(getLogger(), "[CbMoveEndEffector] motion execution failed");
108 this->postFailureEvent();
109 }
110
111 RCLCPP_DEBUG(getLogger(), "[CbMoveEndEffector] Synchronous sleep of 1 seconds");
112 rclcpp::sleep_for(500ms);
113
114 return success;
115}

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

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::CbMoveEndEffector::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 42 of file cb_move_end_effector.cpp.

43{
45
46 if (this->group_)
47 {
48 RCLCPP_DEBUG(
49 getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector");
50 moveit::planning_interface::MoveGroupInterface move_group(getNode(), *group_);
51 this->moveToAbsolutePose(move_group, targetPose);
52 RCLCPP_DEBUG(getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed");
53 }
54 else
55 {
56 RCLCPP_DEBUG(
57 getLogger(), "[CbMoveEndEfector] new thread started to move absolute end effector");
59 RCLCPP_DEBUG(getLogger(), "[CbMoveEndEfector] to move absolute end effector thread destroyed");
60 }
61}
bool moveToAbsolutePose(moveit::planning_interface::MoveGroupInterface &moveGroupInterface, geometry_msgs::msg::PoseStamped &targetObjectPose)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)

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

Here is the call graph for this function:

Member Data Documentation

◆ group_

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

Definition at line 33 of file cb_move_end_effector.hpp.

Referenced by onEntry().

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveEndEffector::movegroupClient_
protected

Definition at line 45 of file cb_move_end_effector.hpp.

Referenced by moveToAbsolutePose(), and onEntry().

◆ targetPose

geometry_msgs::msg::PoseStamped cl_move_group_interface::CbMoveEndEffector::targetPose

Definition at line 31 of file cb_move_end_effector.hpp.

Referenced by onEntry().

◆ tip_link_

std::string cl_move_group_interface::CbMoveEndEffector::tip_link_

Definition at line 32 of file cb_move_end_effector.hpp.

Referenced by CbMoveEndEffector(), and moveToAbsolutePose().


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