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

#include <cb_move_cartesian_relative.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveCartesianRelative:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveCartesianRelative:
Collaboration graph

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 ()
 
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::Vector3 offset_
 
std::optional< double > scalingFactor_
 
std::optional< std::string > group_
 
ClMoveGroupmoveGroupSmaccClient_
 

Additional Inherited Members

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

Detailed Description

Definition at line 30 of file cb_move_cartesian_relative.hpp.

Constructor & Destructor Documentation

◆ CbMoveCartesianRelative() [1/2]

cl_move_group_interface::CbMoveCartesianRelative::CbMoveCartesianRelative ( )

Definition at line 27 of file cb_move_cartesian_relative.cpp.

27{}

◆ CbMoveCartesianRelative() [2/2]

cl_move_group_interface::CbMoveCartesianRelative::CbMoveCartesianRelative ( geometry_msgs::msg::Vector3  offset)

Definition at line 29 of file cb_move_cartesian_relative.cpp.

30: offset_(offset)
31{
32}

Member Function Documentation

◆ moveRelativeCartesian()

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

Definition at line 53 of file cb_move_cartesian_relative.cpp.

56{
57 std::vector<geometry_msgs::msg::Pose> waypoints;
58
59 // this one was working fine but the issue is that for relative motions it grows up on ABORT-State-Loop pattern.
60 // But, we need current pose because maybe setCurrentPose was not set by previous behaviors. The only solution would be
61 // distinguishing between the execution error and the planning error with no state change
62 //auto referenceStartPose = movegroupClient->getPoseTarget();
63 auto referenceStartPose = movegroupClient->getCurrentPose();
64 movegroupClient->setPoseReferenceFrame(referenceStartPose.header.frame_id);
65
66 RCLCPP_INFO_STREAM(
67 getLogger(), "[CbMoveCartesianRelative] RELATIVE MOTION, SOURCE POSE: " << referenceStartPose);
68 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Offset: " << offset);
69
70 waypoints.push_back(referenceStartPose.pose); // up and out
71
72 auto endPose = referenceStartPose.pose;
73
74 endPose.position.x += offset.x;
75 endPose.position.y += offset.y;
76 endPose.position.z += offset.z;
77
78 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] DESTINY POSE: " << endPose);
79
80 // target_pose2.position.x -= 0.15;
81 waypoints.push_back(endPose); // left
82
83 movegroupClient->setPoseTarget(endPose);
84
85 float scalinf = 0.5;
86 if (scalingFactor_) scalinf = *scalingFactor_;
87
88 RCLCPP_INFO_STREAM(getLogger(), "[CbMoveCartesianRelative] Using scaling factor: " << scalinf);
89 movegroupClient->setMaxVelocityScalingFactor(scalinf);
90
91 moveit_msgs::msg::RobotTrajectory trajectory;
92 double fraction = movegroupClient->computeCartesianPath(
93 waypoints,
94 0.01, // eef_step
95 0.00, // jump_threshold
96 trajectory);
97
98 moveit::planning_interface::MoveItErrorCode behaviorResult;
99 if (fraction != 1.0 || fraction == -1)
100 {
101 RCLCPP_WARN_STREAM(
102 getLogger(),
103 "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje. Execution skipped "
104 "because not 100% of cartesian motion: "
105 << fraction * 100 << "%");
106 behaviorResult = moveit::planning_interface::MoveItErrorCode::PLANNING_FAILED;
107 }
108 else
109 {
110 RCLCPP_INFO_STREAM(
111 getLogger(),
112 "[CbMoveCartesianRelative] Cartesian plan joint-continuity percentaje: " << fraction);
113
114 moveit::planning_interface::MoveGroupInterface::Plan grasp_pose_plan;
115
116 // grasp_pose_plan.start_state_ = *(moveGroupInterface.getCurrentState());
117 grasp_pose_plan.trajectory_ = trajectory;
118 behaviorResult = movegroupClient->execute(grasp_pose_plan);
119 }
120
121 if (behaviorResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
122 {
123 RCLCPP_INFO(
124 getLogger(), "[CbMoveCartesianRelative] relative motion execution succeeded: fraction %lf.",
125 fraction);
127 this->postSuccessEvent();
128 }
129 else
130 {
131 movegroupClient->setPoseTarget(
132 referenceStartPose); // undo changes since we did not executed the motion
133 RCLCPP_INFO(
134 getLogger(), "[CbMoveCartesianRelative] relative motion execution failed: fraction %lf.",
135 fraction);
137 this->postFailureEvent();
138 }
139}

References smacc2::ISmaccClientBehavior::getLogger(), moveGroupSmaccClient_, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), cl_move_group_interface::ClMoveGroup::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_move_group_interface::CbMoveCartesianRelative::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 34 of file cb_move_cartesian_relative.cpp.

35{
37
38 if (this->group_)
39 {
40 moveit::planning_interface::MoveGroupInterface move_group(
41 getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
42 this->moveRelativeCartesian(&move_group, offset_);
43 }
44 else
45 {
47 }
48}
void moveRelativeCartesian(moveit::planning_interface::MoveGroupInterface *movegroupClient, geometry_msgs::msg::Vector3 &offset)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)

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

Here is the call graph for this function:

◆ onExit()

void cl_move_group_interface::CbMoveCartesianRelative::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 50 of file cb_move_cartesian_relative.cpp.

50{}

Member Data Documentation

◆ group_

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

Definition at line 37 of file cb_move_cartesian_relative.hpp.

Referenced by onEntry().

◆ moveGroupSmaccClient_

ClMoveGroup* cl_move_group_interface::CbMoveCartesianRelative::moveGroupSmaccClient_

Definition at line 52 of file cb_move_cartesian_relative.hpp.

Referenced by moveRelativeCartesian(), and onEntry().

◆ offset_

geometry_msgs::msg::Vector3 cl_move_group_interface::CbMoveCartesianRelative::offset_

Definition at line 33 of file cb_move_cartesian_relative.hpp.

Referenced by onEntry().

◆ scalingFactor_

std::optional<double> cl_move_group_interface::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: