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

#include <cb_move_cartesian_relative2.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveCartesianRelative2:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveCartesianRelative2:
Collaboration graph

Public Member Functions

 CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink)
 
 CbMoveCartesianRelative2 (std::string referenceFrame, std::string tipLink, geometry_msgs::msg::Vector3 offset)
 
virtual ~CbMoveCartesianRelative2 ()
 
void generateTrajectory () override
 
- Public Member Functions inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
 CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)
 
 CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
virtual void onEntry () override
 
virtual void onExit () override
 
virtual void update () 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 Member Functions inherited from smacc2::ISmaccUpdatable
 ISmaccUpdatable ()
 
 ISmaccUpdatable (rclcpp::Duration duration)
 
void executeUpdate (rclcpp::Node::SharedPtr node)
 
void setUpdatePeriod (rclcpp::Duration duration)
 

Public Attributes

geometry_msgs::msg::Vector3 offset_
 
std::optional< double > linearSpeed_m_s_
 
- Public Attributes inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
std::optional< std::string > group_
 
std::optional< std::string > tipLink_
 
std::optional< boolallowInitialTrajectoryStateJointDiscontinuity_
 

Private Attributes

std::string globalFrame_
 

Additional Inherited Members

- Protected Member Functions inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
ComputeJointTrajectoryErrorCode computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
 
void executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
 
virtual void generateTrajectory ()=0
 
virtual void createMarkers ()
 
void getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
 
- 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 ()
 
virtual void update ()=0
 
- Protected Attributes inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::msg::PoseStamped > endEffectorTrajectory_
 
ClMoveGroupmovegroupClient_ = nullptr
 
visualization_msgs::msg::MarkerArray beahiorMarkers_
 

Detailed Description

Definition at line 31 of file cb_move_cartesian_relative2.hpp.

Constructor & Destructor Documentation

◆ CbMoveCartesianRelative2() [1/2]

cl_move_group_interface::CbMoveCartesianRelative2::CbMoveCartesianRelative2 ( std::string  referenceFrame,
std::string  tipLink 
)

Definition at line 28 of file cb_move_cartesian_relative2.cpp.

30{
31 globalFrame_ = referenceFrame;
32}
CbMoveEndEffectorTrajectory(std::optional< std::string > tipLink=std::nullopt)

References globalFrame_.

◆ CbMoveCartesianRelative2() [2/2]

cl_move_group_interface::CbMoveCartesianRelative2::CbMoveCartesianRelative2 ( std::string  referenceFrame,
std::string  tipLink,
geometry_msgs::msg::Vector3  offset 
)

Definition at line 34 of file cb_move_cartesian_relative2.cpp.

37{
38 globalFrame_ = referenceFrame;
39 offset_ = offset;
40}

References globalFrame_, and offset_.

◆ ~CbMoveCartesianRelative2()

cl_move_group_interface::CbMoveCartesianRelative2::~CbMoveCartesianRelative2 ( )
virtual

Definition at line 42 of file cb_move_cartesian_relative2.cpp.

42{}

Member Function Documentation

◆ generateTrajectory()

void cl_move_group_interface::CbMoveCartesianRelative2::generateTrajectory ( )
overridevirtual

Implements cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 44 of file cb_move_cartesian_relative2.cpp.

45{
46 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] generating trajectory");
47 // at least 1 sample per centimeter (average)
48 const double METERS_PER_SAMPLE = 0.001;
49
50 float dist_meters = 0;
51 tf2::Vector3 voffset;
52 tf2::fromMsg(offset_, voffset);
53
54 float totallineardist = voffset.length();
55
56 int totalSamplesCount = totallineardist / METERS_PER_SAMPLE;
57 int steps = totallineardist / METERS_PER_SAMPLE;
58
59 float interpolation_factor_step = 1.0 / totalSamplesCount;
60
61 double secondsPerSample;
62
64 {
65 linearSpeed_m_s_ = 0.1;
66 }
67
68 secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
69
70 tf2::Stamped<tf2::Transform> currentEndEffectorTransform;
71 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current end effector pose");
72 this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
73
74 tf2::Transform finalEndEffectorTransform = currentEndEffectorTransform;
75 finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset);
76
77 float linc = totallineardist / steps; // METERS_PER_SAMPLE with sign
78 float interpolation_factor = 0;
79
80 rclcpp::Time startTime = getNode()->now();
81 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps);
82
83 for (float i = 0; i < steps; i++)
84 {
85 interpolation_factor += interpolation_factor_step;
86 dist_meters += linc;
87
88 tf2::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp(
89 finalEndEffectorTransform.getOrigin(), interpolation_factor);
90
91 tf2::Transform pose;
92 pose.setOrigin(vi);
93 pose.setRotation(currentEndEffectorTransform.getRotation());
94
95 geometry_msgs::msg::PoseStamped pointerPose;
96 tf2::toMsg(pose, pointerPose.pose);
97 pointerPose.header.frame_id = globalFrame_;
98 pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample);
99
100 this->endEffectorTrajectory_.push_back(pointerPose);
101 }
102
103 RCLCPP_INFO_STREAM(
104 getLogger(),
105 "[" << getName() << "] Target End efector Pose: " << this->endEffectorTrajectory_.back());
106}
void getCurrentEndEffectorPose(std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
std::vector< geometry_msgs::msg::PoseStamped > endEffectorTrajectory_
virtual rclcpp::Node::SharedPtr getNode()

References cl_move_group_interface::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), globalFrame_, linearSpeed_m_s_, and offset_.

Here is the call graph for this function:

Member Data Documentation

◆ globalFrame_

std::string cl_move_group_interface::CbMoveCartesianRelative2::globalFrame_
private

Definition at line 48 of file cb_move_cartesian_relative2.hpp.

Referenced by CbMoveCartesianRelative2(), and generateTrajectory().

◆ linearSpeed_m_s_

std::optional<double> cl_move_group_interface::CbMoveCartesianRelative2::linearSpeed_m_s_

Definition at line 36 of file cb_move_cartesian_relative2.hpp.

Referenced by generateTrajectory().

◆ offset_

geometry_msgs::msg::Vector3 cl_move_group_interface::CbMoveCartesianRelative2::offset_

Definition at line 34 of file cb_move_cartesian_relative2.hpp.

Referenced by CbMoveCartesianRelative2(), and generateTrajectory().


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