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

#include <cb_move_last_trajectory_initial_state.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveLastTrajectoryInitialState:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveLastTrajectoryInitialState:
Collaboration graph

Public Member Functions

 CbMoveLastTrajectoryInitialState ()
 
 CbMoveLastTrajectoryInitialState (int backIndex)
 
virtual ~CbMoveLastTrajectoryInitialState ()
 
virtual void onEntry () override
 
- Public Member Functions inherited from cl_move_group_interface::CbMoveJoints
 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)
 
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 ()
 

Private Attributes

int backIndex_ = -1
 

Additional Inherited Members

- Public Attributes inherited from cl_move_group_interface::CbMoveJoints
std::optional< double > scalingFactor_
 
std::map< std::string, double > jointValueTarget_
 
std::optional< std::string > group_
 
- Protected Member Functions inherited from cl_move_group_interface::CbMoveJoints
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 ()
 
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 inherited from cl_move_group_interface::CbMoveJoints
ClMoveGroupmovegroupClient_
 

Detailed Description

Definition at line 27 of file cb_move_last_trajectory_initial_state.hpp.

Constructor & Destructor Documentation

◆ CbMoveLastTrajectoryInitialState() [1/2]

cl_move_group_interface::CbMoveLastTrajectoryInitialState::CbMoveLastTrajectoryInitialState ( )

Definition at line 26 of file cb_move_last_trajectory_initial_state.cpp.

26{}

◆ CbMoveLastTrajectoryInitialState() [2/2]

cl_move_group_interface::CbMoveLastTrajectoryInitialState::CbMoveLastTrajectoryInitialState ( int  backIndex)

◆ ~CbMoveLastTrajectoryInitialState()

cl_move_group_interface::CbMoveLastTrajectoryInitialState::~CbMoveLastTrajectoryInitialState ( )
virtual

Definition at line 33 of file cb_move_last_trajectory_initial_state.cpp.

33{}

Member Function Documentation

◆ onEntry()

void cl_move_group_interface::CbMoveLastTrajectoryInitialState::onEntry ( )
overridevirtual

Reimplemented from cl_move_group_interface::CbMoveJoints.

Definition at line 35 of file cb_move_last_trajectory_initial_state.cpp.

36{
37 CpTrajectoryHistory * trajectoryHistory;
38 this->requiresComponent(trajectoryHistory);
39
40 if (trajectoryHistory != nullptr)
41 {
42 moveit_msgs::msg::RobotTrajectory trajectory;
43
44 bool trajectoryFound = trajectoryHistory->getLastTrajectory(backIndex_, trajectory);
45
46 if (trajectoryFound)
47 {
48 trajectory_msgs::msg::JointTrajectoryPoint & initialPoint =
49 trajectory.joint_trajectory.points.front();
50
51 std::stringstream ss;
52 for (size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); i++)
53 {
54 auto & name = trajectory.joint_trajectory.joint_names[i];
55
56 jointValueTarget_[name] = initialPoint.positions[i];
57 ss << name << ": " << jointValueTarget_[name] << std::endl;
58 }
59 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "]" << std::endl << ss.str());
60
61 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry");
63 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry finished");
64 }
65 }
66
67 //call base OnEntry
68}
std::map< std::string, double > jointValueTarget_
virtual rclcpp::Logger getLogger() const
void requiresComponent(SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)

References backIndex_, cl_move_group_interface::CpTrajectoryHistory::getLastTrajectory(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_move_group_interface::CbMoveJoints::jointValueTarget_, cl_move_group_interface::CbMoveJoints::onEntry(), and smacc2::ISmaccClientBehavior::requiresComponent().

Here is the call graph for this function:

Member Data Documentation

◆ backIndex_

int cl_move_group_interface::CbMoveLastTrajectoryInitialState::backIndex_ = -1
private

Definition at line 39 of file cb_move_last_trajectory_initial_state.hpp.

Referenced by onEntry().


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