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

#include <cb_move_joints.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveJoints:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveJoints:
Collaboration graph

Public Member Functions

 CbMoveJoints ()
 
 CbMoveJoints (const std::map< std::string, double > &jointValueTarget)
 
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)
 
- 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

std::optional< double > scalingFactor_
 
std::map< std::string, double > jointValueTarget_
 
std::optional< std::string > group_
 

Protected Member Functions

void moveJoints (moveit::planning_interface::MoveGroupInterface &moveGroupInterface)
 
- 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 31 of file cb_move_joints.hpp.

Constructor & Destructor Documentation

◆ CbMoveJoints() [1/2]

cl_move_group_interface::CbMoveJoints::CbMoveJoints ( )

Definition at line 31 of file cb_move_joints.cpp.

31{}

◆ CbMoveJoints() [2/2]

cl_move_group_interface::CbMoveJoints::CbMoveJoints ( const std::map< std::string, double > &  jointValueTarget)

Definition at line 26 of file cb_move_joints.cpp.

27: jointValueTarget_(jointValueTarget)
28{
29}
std::map< std::string, double > jointValueTarget_

Member Function Documentation

◆ moveJoints()

void cl_move_group_interface::CbMoveJoints::moveJoints ( moveit::planning_interface::MoveGroupInterface &  moveGroupInterface)
protected

Definition at line 72 of file cb_move_joints.cpp.

73{
74 if (scalingFactor_) moveGroupInterface.setMaxVelocityScalingFactor(*scalingFactor_);
75
76 bool success;
77 moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
78
79 if (jointValueTarget_.size() == 0)
80 {
81 RCLCPP_WARN(
82 getLogger(), "[CbMoveJoints] No joint was value specified. Skipping planning call.");
83 success = false;
84 }
85 else
86 {
87 moveGroupInterface.setJointValueTarget(jointValueTarget_);
88 //moveGroupInterface.setGoalJointTolerance(0.01);
89 success =
90 (moveGroupInterface.plan(computedMotionPlan) ==
91 moveit::planning_interface::MoveItErrorCode::SUCCESS);
92 RCLCPP_INFO(getLogger(), "Success Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
93 }
94
95 if (success)
96 {
97 auto executionResult = moveGroupInterface.execute(computedMotionPlan);
98
99 auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
100
101 if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
102 {
103 RCLCPP_INFO_STREAM(
104 getLogger(), "[" << this->getName()
105 << "] motion execution succeeded. Throwing success event. " << std::endl
106 << statestr);
108 this->postSuccessEvent();
109 }
110 else
111 {
112 RCLCPP_WARN_STREAM(
113 getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
114 << std::endl
115 << statestr);
117 this->postFailureEvent();
118 }
119 }
120 else
121 {
122 auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
123 RCLCPP_WARN_STREAM(
124 getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
125 << std::endl
126 << statestr);
128 this->postFailureEvent();
129 }
130}
std::optional< double > scalingFactor_
std::string currentJointStatesToString(moveit::planning_interface::MoveGroupInterface &moveGroupInterface, std::map< std::string, double > &targetJoints)

References cl_move_group_interface::currentJointStatesToString(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), jointValueTarget_, movegroupClient_, 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::CbMoveJoints::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_move_group_interface::CbMoveKnownState, and cl_move_group_interface::CbMoveLastTrajectoryInitialState.

Definition at line 33 of file cb_move_joints.cpp.

34{
36
37 if (this->group_)
38 {
39 moveit::planning_interface::MoveGroupInterface move_group(
40 getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
41 this->moveJoints(move_group);
42 }
43 else
44 {
46 }
47}
std::optional< std::string > group_
void moveJoints(moveit::planning_interface::MoveGroupInterface &moveGroupInterface)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)

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

Referenced by cl_move_group_interface::CbMoveKnownState::onEntry(), and cl_move_group_interface::CbMoveLastTrajectoryInitialState::onEntry().

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

◆ onExit()

void cl_move_group_interface::CbMoveJoints::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 132 of file cb_move_joints.cpp.

132{}

Member Data Documentation

◆ group_

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

Definition at line 36 of file cb_move_joints.hpp.

Referenced by onEntry().

◆ jointValueTarget_

std::map<std::string, double> cl_move_group_interface::CbMoveJoints::jointValueTarget_

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveJoints::movegroupClient_
protected

Definition at line 45 of file cb_move_joints.hpp.

Referenced by moveJoints(), and onEntry().

◆ scalingFactor_

std::optional<double> cl_move_group_interface::CbMoveJoints::scalingFactor_

Definition at line 34 of file cb_move_joints.hpp.

Referenced by moveJoints().


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