SMACC2
Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
cl_move_group_interface::CbMoveEndEffectorTrajectory Class Referenceabstract

#include <cb_move_end_effector_trajectory.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveEndEffectorTrajectory:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveEndEffectorTrajectory:
Collaboration graph

Public Member Functions

 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

std::optional< std::string > group_
 
std::optional< std::string > tipLink_
 
std::optional< boolallowInitialTrajectoryStateJointDiscontinuity_
 

Protected Member Functions

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

std::vector< geometry_msgs::msg::PoseStamped > endEffectorTrajectory_
 
ClMoveGroupmovegroupClient_ = nullptr
 
visualization_msgs::msg::MarkerArray beahiorMarkers_
 

Private Member Functions

void initializeROS ()
 

Private Attributes

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr markersPub_
 
std::atomic< boolmarkersInitialized_
 
rclcpp::Client< moveit_msgs::srv::GetPositionIK >::SharedPtr iksrv_
 
std::mutex m_mutex_
 
std::function< void(moveit_msgs::msg::RobotTrajectory &)> postJointDiscontinuityEvent
 
std::function< void(moveit_msgs::msg::RobotTrajectory &)> postIncorrectInitialStateEvent
 
std::function< void()> postMotionExecutionFailureEvents
 
bool autocleanmarkers = false
 

Detailed Description

Definition at line 51 of file cb_move_end_effector_trajectory.hpp.

Constructor & Destructor Documentation

◆ CbMoveEndEffectorTrajectory() [1/2]

cl_move_group_interface::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( std::optional< std::string >  tipLink = std::nullopt)

◆ CbMoveEndEffectorTrajectory() [2/2]

cl_move_group_interface::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( const std::vector< geometry_msgs::msg::PoseStamped > &  endEffectorTrajectory,
std::optional< std::string >  tipLink = std::nullopt 
)

Definition at line 40 of file cb_move_end_effector_trajectory.cpp.

43: tipLink_(tipLink), endEffectorTrajectory_(endEffectorTrajectory), markersInitialized_(false)
44
45{
46}
std::vector< geometry_msgs::msg::PoseStamped > endEffectorTrajectory_

Member Function Documentation

◆ computeJointSpaceTrajectory()

ComputeJointTrajectoryErrorCode cl_move_group_interface::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory ( moveit_msgs::msg::RobotTrajectory &  computedJointTrajectory)
protected

Definition at line 57 of file cb_move_end_effector_trajectory.cpp.

59{
60 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current state.. waiting");
61
62 // get current robot state
63 auto currentState = movegroupClient_->moveGroupClientInterface->getCurrentState(100);
64
65 // get the IK client
66 auto groupname = movegroupClient_->moveGroupClientInterface->getName();
67
68 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting joint names");
69 auto currentjointnames = currentState->getJointModelGroup(groupname)->getActiveJointModelNames();
70
71 if (!tipLink_ || *tipLink_ == "")
72 {
73 tipLink_ = movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
74 }
75
76 std::vector<double> jointPositions;
77 currentState->copyJointGroupPositions(groupname, jointPositions);
78
79 std::vector<std::vector<double>> trajectory;
80 std::vector<rclcpp::Duration> trajectoryTimeStamps;
81
82 trajectory.push_back(jointPositions);
83 trajectoryTimeStamps.push_back(rclcpp::Duration(0s));
84
85 auto & first = endEffectorTrajectory_.front();
86 rclcpp::Time referenceTime(first.header.stamp);
87
88 std::vector<int> discontinuityIndexes;
89
90 int ikAttempts = 4;
91 for (size_t k = 0; k < this->endEffectorTrajectory_.size(); k++)
92 {
93 auto & pose = this->endEffectorTrajectory_[k];
94 auto req = std::make_shared<moveit_msgs::srv::GetPositionIK::Request>();
95 //req.ik_request.attempts = 20;
96
97 req->ik_request.ik_link_name = *tipLink_;
98 req->ik_request.robot_state.joint_state.name = currentjointnames;
99 req->ik_request.robot_state.joint_state.position = jointPositions;
100
101 req->ik_request.group_name = groupname;
102 req->ik_request.avoid_collisions = true;
103
104 //pose.header.stamp = getNode()->now();
105 req->ik_request.pose_stamped = pose;
106
107 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] IK request: " << k << " " << *req);
108
109 auto resfut = iksrv_->async_send_request(req);
110
111 auto status = resfut.wait_for(3s);
112 if (status == std::future_status::ready)
113 {
114 //if (rclcpp::spin_until_future_complete(getNode(), resfut) == rclcpp::FutureReturnCode::SUCCESS)
115 //{
116 auto & prevtrajpoint = trajectory.back();
117 //jointPositions.clear();
118
119 auto res = resfut.get();
120 std::stringstream ss;
121 for (size_t j = 0; j < res->solution.joint_state.position.size(); j++)
122 {
123 auto & jointname = res->solution.joint_state.name[j];
124 auto it = std::find(currentjointnames.begin(), currentjointnames.end(), jointname);
125 if (it != currentjointnames.end())
126 {
127 int index = std::distance(currentjointnames.begin(), it);
128 jointPositions[index] = res->solution.joint_state.position[j];
129 ss << jointname << "(" << index << "): " << jointPositions[index] << std::endl;
130 }
131 }
132
133 // continuity check
134 size_t jointindex = 0;
135 int discontinuityJointIndex = -1;
136 double discontinuityDeltaJointIndex = -1;
137 double deltajoint;
138
139 bool check = k > 0 || !allowInitialTrajectoryStateJointDiscontinuity_ ||
141 !(*allowInitialTrajectoryStateJointDiscontinuity_));
142 if (check)
143 {
144 for (jointindex = 0; jointindex < jointPositions.size(); jointindex++)
145 {
146 deltajoint = jointPositions[jointindex] - prevtrajpoint[jointindex];
147
148 if (fabs(deltajoint) > 0.3 /*2.5 deg*/)
149 {
150 discontinuityDeltaJointIndex = deltajoint;
151 discontinuityJointIndex = jointindex;
152 }
153 }
154 }
155
156 if (ikAttempts > 0 && discontinuityJointIndex != -1)
157 {
158 k--;
159 ikAttempts--;
160 continue;
161 }
162 else
163 {
164 bool discontinuity = false;
165 if (ikAttempts == 0)
166 {
167 discontinuityIndexes.push_back(k);
168 discontinuity = true;
169 }
170
171 ikAttempts = 4;
172
173 if (discontinuity && discontinuityJointIndex != -1)
174 {
175 // show a message and stop the trajectory generation && jointindex!= 7 || fabs(deltajoint) > 0.1 /*2.5 deg*/ && jointindex== 7
176 std::stringstream ss;
177 ss << "Traj[" << k << "/" << endEffectorTrajectory_.size() << "] "
178 << currentjointnames[discontinuityJointIndex]
179 << " IK discontinuity : " << discontinuityDeltaJointIndex << std::endl
180 << "prev joint value: " << prevtrajpoint[discontinuityJointIndex] << std::endl
181 << "current joint value: " << jointPositions[discontinuityJointIndex] << std::endl;
182
183 ss << std::endl;
184 for (size_t ji = 0; ji < jointPositions.size(); ji++)
185 {
186 ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl;
187 }
188
189 for (size_t kindex = 0; kindex < trajectory.size(); kindex++)
190 {
191 ss << "[" << kindex << "]: " << trajectory[kindex][discontinuityJointIndex]
192 << std::endl;
193 }
194
195 if (k == 0)
196 {
197 ss << "This is the first posture of the trajectory. Maybe the robot initial posture is "
198 "not coincident to the initial posture of the generated joint trajectory."
199 << std::endl;
200 }
201
202 RCLCPP_ERROR_STREAM(getLogger(), ss.str());
203
204 trajectory.push_back(jointPositions);
205 rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime;
206 trajectoryTimeStamps.push_back(durationFromStart);
207
208 continue;
209 }
210 else
211 {
212 trajectory.push_back(jointPositions);
213 rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime;
214 trajectoryTimeStamps.push_back(durationFromStart);
215
216 RCLCPP_DEBUG_STREAM(getLogger(), "IK solution: " << res->solution.joint_state);
217 RCLCPP_DEBUG_STREAM(getLogger(), "trajpoint: " << std::endl << ss.str());
218 }
219 }
220 }
221 else
222 {
223 RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] wrong IK call");
224 }
225 }
226
227 // interpolate speeds?
228
229 // interpolate accelerations?
230
231 // get current robot state
232 // fill plan message
233 // computedMotionPlan.start_state_.joint_state.name = currentjointnames;
234 // computedMotionPlan.start_state_.joint_state.position = trajectory.front();
235 // computedMotionPlan.trajectory_.joint_trajectory.joint_names = currentjointnames;
236
237 computedJointTrajectory.joint_trajectory.joint_names = currentjointnames;
238 int i = 0;
239 for (auto & p : trajectory)
240 {
241 if (
242 i ==
243 0) // not copy the current state in the trajectory (used to solve discontinuity in other behaviors)
244 {
245 i++;
246 continue;
247 }
248
249 trajectory_msgs::msg::JointTrajectoryPoint jp;
250 jp.positions = p;
251 jp.time_from_start = trajectoryTimeStamps[i]; //rclcpp::Duration(t);
252 computedJointTrajectory.joint_trajectory.points.push_back(jp);
253 i++;
254 }
255
256 if (discontinuityIndexes.size())
257 {
258 if (discontinuityIndexes[0] == 0)
260 else
262 }
263
265}
rclcpp::Client< moveit_msgs::srv::GetPositionIK >::SharedPtr iksrv_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface

References allowInitialTrajectoryStateJointDiscontinuity_, smacc2::ISmaccClientBehavior::currentState, endEffectorTrajectory_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), iksrv_, cl_move_group_interface::INCORRECT_INITIAL_STATE, cl_move_group_interface::JOINT_TRAJECTORY_DISCONTINUITY, movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, service_node_3::s, cl_move_group_interface::SUCCESS, and tipLink_.

Referenced by onEntry().

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

◆ createMarkers()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::createMarkers ( )
protectedvirtual

Reimplemented in cl_move_group_interface::CbCircularPivotMotion, and cl_move_group_interface::CbCircularPouringMotion.

Definition at line 384 of file cb_move_end_effector_trajectory.cpp.

385{
386 tf2::Transform localdirection;
387 localdirection.setIdentity();
388 localdirection.setOrigin(tf2::Vector3(0.05, 0, 0));
389 auto frameid = this->endEffectorTrajectory_.front().header.frame_id;
390
391 for (auto & pose : this->endEffectorTrajectory_)
392 {
393 visualization_msgs::msg::Marker marker;
394 marker.header.frame_id = frameid;
395 marker.header.stamp = getNode()->now();
396 marker.ns = "trajectory";
397 marker.id = this->beahiorMarkers_.markers.size();
398 marker.type = visualization_msgs::msg::Marker::ARROW;
399 marker.action = visualization_msgs::msg::Marker::ADD;
400 marker.scale.x = 0.005;
401 marker.scale.y = 0.01;
402 marker.scale.z = 0.01;
403 marker.color.a = 0.8;
404 marker.color.r = 1.0;
405 marker.color.g = 0;
406 marker.color.b = 0;
407
408 geometry_msgs::msg::Point start, end;
409 start.x = 0;
410 start.y = 0;
411 start.z = 0;
412
413 tf2::Transform basetransform;
414 tf2::fromMsg(pose.pose, basetransform);
415 // tf2::Transform endarrow = localdirection * basetransform;
416
417 end.x = localdirection.getOrigin().x();
418 end.y = localdirection.getOrigin().y();
419 end.z = localdirection.getOrigin().z();
420
421 marker.pose.position = pose.pose.position;
422 marker.pose.orientation = pose.pose.orientation;
423 marker.points.push_back(start);
424 marker.points.push_back(end);
425
426 beahiorMarkers_.markers.push_back(marker);
427 }
428}
virtual rclcpp::Node::SharedPtr getNode()

References beahiorMarkers_, endEffectorTrajectory_, and smacc2::ISmaccClientBehavior::getNode().

Referenced by cl_move_group_interface::CbCircularPivotMotion::createMarkers(), cl_move_group_interface::CbCircularPouringMotion::createMarkers(), and onEntry().

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

◆ executeJointSpaceTrajectory()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::executeJointSpaceTrajectory ( const moveit_msgs::msg::RobotTrajectory &  computedJointTrajectory)
protected

Definition at line 267 of file cb_move_end_effector_trajectory.cpp.

269{
270 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory");
271 // call execute
272 auto executionResult =
273 this->movegroupClient_->moveGroupClientInterface->execute(computedJointTrajectory);
274
275 if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
276 {
277 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution succeeded");
279 this->postSuccessEvent();
280 }
281 else
282 {
284 this->postFailureEvent();
285 }
286}

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, and smacc2::SmaccAsyncClientBehavior::postSuccessEvent().

Referenced by cl_move_group_interface::CbExecuteLastTrajectory::onEntry(), onEntry(), and cl_move_group_interface::CbUndoLastTrajectory::onEntry().

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

◆ generateTrajectory()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::generateTrajectory ( )
protectedpure virtual

Implemented in cl_move_group_interface::CbExecuteLastTrajectory, cl_move_group_interface::CbUndoLastTrajectory, cl_move_group_interface::CbCircularPivotMotion, cl_move_group_interface::CbMoveCartesianRelative2, and cl_move_group_interface::CbCircularPouringMotion.

Definition at line 430 of file cb_move_end_effector_trajectory.cpp.

431{
432 // bypass current trajectory, overridden in derived classes
433 // this->endEffectorTrajectory_ = ...
434}

Referenced by onEntry().

Here is the caller graph for this function:

◆ getCurrentEndEffectorPose()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose ( std::string  globalFrame,
tf2::Stamped< tf2::Transform > &  currentEndEffectorTransform 
)
protected

Definition at line 436 of file cb_move_end_effector_trajectory.cpp.

438{
439 tf2_ros::Buffer tfBuffer(getNode()->get_clock());
440 tf2_ros::TransformListener tfListener(tfBuffer);
441
442 try
443 {
444 if (!tipLink_ || *tipLink_ == "")
445 {
446 tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
447 }
448
449 tf2::fromMsg(
450 tfBuffer.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), rclcpp::Duration(10s)),
451 currentEndEffectorTransform);
452
453 //tfListener.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), currentEndEffectorTransform);
454
455 // we define here the global frame as the pivot frame id
456 // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10));
457 // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink);
458 }
459 catch (const std::exception & e)
460 {
461 std::cerr << e.what() << std::endl;
462 }
463}

References smacc2::ISmaccClientBehavior::getNode(), movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, service_node_3::s, and tipLink_.

Referenced by cl_move_group_interface::CbCircularPouringMotion::createMarkers(), cl_move_group_interface::CbMoveCartesianRelative2::generateTrajectory(), and cl_move_group_interface::CbCircularPouringMotion::generateTrajectory().

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

◆ initializeROS()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::initializeROS ( )
private

Definition at line 48 of file cb_move_end_effector_trajectory.cpp.

49{
50 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] initializing ros");
51
52 auto nh = this->getNode();
53 markersPub_ = nh->create_publisher<visualization_msgs::msg::MarkerArray>("trajectory_markers", 1);
54 iksrv_ = nh->create_client<moveit_msgs::srv::GetPositionIK>("/compute_ik");
55}
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr markersPub_

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), iksrv_, and markersPub_.

Referenced by onOrthogonalAllocation().

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

◆ onEntry()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onEntry ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_move_group_interface::CbEndEffectorRotate, cl_move_group_interface::CbExecuteLastTrajectory, and cl_move_group_interface::CbUndoLastTrajectory.

Definition at line 288 of file cb_move_end_effector_trajectory.cpp.

289{
291
292 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Generating end effector trajectory");
293
294 this->generateTrajectory();
295
296 if (this->endEffectorTrajectory_.size() == 0)
297 {
298 RCLCPP_WARN_STREAM(
299 getLogger(), "[" << smacc2::demangleSymbol(typeid(*this).name())
300 << "] No points in the trajectory. Skipping behavior.");
301 return;
302 }
303
304 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating markers.");
305
306 this->createMarkers();
307 markersInitialized_ = true;
308 moveit_msgs::msg::RobotTrajectory computedTrajectory;
309
310 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Computing joint space trajectory.");
311
312 auto errorcode = computeJointSpaceTrajectory(computedTrajectory);
313
314 bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS;
315
316 CpTrajectoryHistory * trajectoryHistory;
317 this->requiresComponent(trajectoryHistory);
318
319 if (!trajectoryGenerationSuccess)
320 {
321 RCLCPP_INFO_STREAM(
322 getLogger(), "[" << this->getName() << "] Incorrect trajectory. Posting failure event.");
323 if (trajectoryHistory != nullptr)
324 {
325 moveit_msgs::msg::MoveItErrorCodes error;
326 error.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
327 trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
328 }
329
331 this->postFailureEvent();
332
334 {
335 this->postJointDiscontinuityEvent(computedTrajectory);
336 }
338 {
339 this->postIncorrectInitialStateEvent(computedTrajectory);
340 }
341 return;
342 }
343 else
344 {
345 if (trajectoryHistory != nullptr)
346 {
347 moveit_msgs::msg::MoveItErrorCodes error;
348 error.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
349 trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
350 }
351
352 this->executeJointSpaceTrajectory(computedTrajectory);
353 }
354
355 // handle finishing events
356} // namespace cl_move_group_interface
ComputeJointTrajectoryErrorCode computeJointSpaceTrajectory(moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
std::function< void(moveit_msgs::msg::RobotTrajectory &)> postIncorrectInitialStateEvent
std::function< void(moveit_msgs::msg::RobotTrajectory &)> postJointDiscontinuityEvent
void executeJointSpaceTrajectory(const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void requiresClient(SmaccClientType *&storage)
void requiresComponent(SmaccComponentType *&storage)
std::string demangleSymbol(const std::string &name)

References computeJointSpaceTrajectory(), createMarkers(), smacc2::introspection::demangleSymbol(), endEffectorTrajectory_, executeJointSpaceTrajectory(), generateTrajectory(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_move_group_interface::INCORRECT_INITIAL_STATE, cl_move_group_interface::JOINT_TRAJECTORY_DISCONTINUITY, markersInitialized_, movegroupClient_, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, cl_move_group_interface::CpTrajectoryHistory::pushTrajectory(), smacc2::ISmaccClientBehavior::requiresClient(), smacc2::ISmaccClientBehavior::requiresComponent(), and cl_move_group_interface::SUCCESS.

Referenced by cl_move_group_interface::CbEndEffectorRotate::onEntry().

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

◆ onExit()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onExit ( )
overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 367 of file cb_move_end_effector_trajectory.cpp.

368{
369 markersInitialized_ = false;
370
372 {
373 std::lock_guard<std::mutex> guard(m_mutex_);
374 for (auto & marker : this->beahiorMarkers_.markers)
375 {
376 marker.header.stamp = getNode()->now();
377 marker.action = visualization_msgs::msg::Marker::DELETE;
378 }
379
381 }
382}

References autocleanmarkers, beahiorMarkers_, smacc2::ISmaccClientBehavior::getNode(), m_mutex_, markersInitialized_, and markersPub_.

Here is the call graph for this function:

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >
void cl_move_group_interface::CbMoveEndEffectorTrajectory::onOrthogonalAllocation ( )
inline

Definition at line 69 of file cb_move_end_effector_trajectory.hpp.

70 {
71 this->initializeROS();
72
73 smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
74
75 postJointDiscontinuityEvent = [this](auto traj) {
76 auto ev = new EvJointDiscontinuity<TSourceObject, TOrthogonal>();
77 ev->trajectory = traj;
78 this->postEvent(ev);
79 };
80
81 postIncorrectInitialStateEvent = [this](auto traj) {
82 auto ev = new EvIncorrectInitialPosition<TSourceObject, TOrthogonal>();
83 ev->trajectory = traj;
84 this->postEvent(ev);
85 };
86
88 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution failed");
90 this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject, TOrthogonal>>();
91 };
92 }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), initializeROS(), movegroupClient_, smacc2::ISmaccClientBehavior::postEvent(), cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, and postMotionExecutionFailureEvents.

Here is the call graph for this function:

◆ update()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::update ( )
overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 358 of file cb_move_end_effector_trajectory.cpp.

359{
361 {
362 std::lock_guard<std::mutex> guard(m_mutex_);
364 }
365}

References beahiorMarkers_, m_mutex_, markersInitialized_, and markersPub_.

Member Data Documentation

◆ allowInitialTrajectoryStateJointDiscontinuity_

std::optional<bool> cl_move_group_interface::CbMoveEndEffectorTrajectory::allowInitialTrajectoryStateJointDiscontinuity_

Definition at line 60 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory().

◆ autocleanmarkers

bool cl_move_group_interface::CbMoveEndEffectorTrajectory::autocleanmarkers = false
private

Definition at line 136 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit().

◆ beahiorMarkers_

visualization_msgs::msg::MarkerArray cl_move_group_interface::CbMoveEndEffectorTrajectory::beahiorMarkers_
protected

◆ endEffectorTrajectory_

std::vector<geometry_msgs::msg::PoseStamped> cl_move_group_interface::CbMoveEndEffectorTrajectory::endEffectorTrajectory_
protected

◆ group_

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

Definition at line 56 of file cb_move_end_effector_trajectory.hpp.

◆ iksrv_

rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr cl_move_group_interface::CbMoveEndEffectorTrajectory::iksrv_
private

◆ m_mutex_

std::mutex cl_move_group_interface::CbMoveEndEffectorTrajectory::m_mutex_
private

Definition at line 129 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit(), and update().

◆ markersInitialized_

std::atomic<bool> cl_move_group_interface::CbMoveEndEffectorTrajectory::markersInitialized_
private

Definition at line 125 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), onExit(), and update().

◆ markersPub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr cl_move_group_interface::CbMoveEndEffectorTrajectory::markersPub_
private

Definition at line 123 of file cb_move_end_effector_trajectory.hpp.

Referenced by initializeROS(), onExit(), and update().

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveEndEffectorTrajectory::movegroupClient_ = nullptr
protected

◆ postIncorrectInitialStateEvent

std::function<void(moveit_msgs::msg::RobotTrajectory &)> cl_move_group_interface::CbMoveEndEffectorTrajectory::postIncorrectInitialStateEvent
private

Definition at line 132 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postJointDiscontinuityEvent

std::function<void(moveit_msgs::msg::RobotTrajectory &)> cl_move_group_interface::CbMoveEndEffectorTrajectory::postJointDiscontinuityEvent
private

Definition at line 131 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postMotionExecutionFailureEvents

std::function<void()> cl_move_group_interface::CbMoveEndEffectorTrajectory::postMotionExecutionFailureEvents
private

◆ tipLink_

std::optional<std::string> cl_move_group_interface::CbMoveEndEffectorTrajectory::tipLink_

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