SMACC
Loading...
Searching...
No Matches
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 Reference

#include <cb_move_end_effector_trajectory.h>

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::string tipLink="")
 
 CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::PoseStamped > &endEffectorTrajectory, std::string tipLink="")
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
virtual void onEntry () override
 
virtual void onExit () override
 
virtual void update () override
 
- Public Member Functions inherited from smacc::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 smacc::ISmaccClientBehavior
 ISmaccClientBehavior ()
 
virtual ~ISmaccClientBehavior ()
 
ISmaccStateMachinegetStateMachine ()
 
std::string getName () const
 
template<typename SmaccClientType >
void requiresClient (SmaccClientType *&storage)
 
template<typename SmaccComponentType >
void requiresComponent (SmaccComponentType *&storage)
 
ros::NodeHandle getNode ()
 
- Public Member Functions inherited from smacc::ISmaccUpdatable
 ISmaccUpdatable ()
 
 ISmaccUpdatable (ros::Duration duration)
 
void executeUpdate ()
 
void setUpdatePeriod (ros::Duration duration)
 

Public Attributes

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

Protected Member Functions

ComputeJointTrajectoryErrorCode computeJointSpaceTrajectory (moveit_msgs::RobotTrajectory &computedJointTrajectory)
 
void executeJointSpaceTrajectory (const moveit_msgs::RobotTrajectory &computedJointTrajectory)
 
virtual void generateTrajectory ()
 
virtual void createMarkers ()
 
void getCurrentEndEffectorPose (std::string globalFrame, tf::StampedTransform &currentEndEffectorTransform)
 
- Protected Member Functions inherited from smacc::SmaccAsyncClientBehavior
virtual void executeOnEntry () override
 
virtual void executeOnExit () override
 
void postSuccessEvent ()
 
void postFailureEvent ()
 
virtual void dispose () override
 
- Protected Member Functions inherited from smacc::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 executeOnEntry ()
 
virtual void executeOnExit ()
 
virtual void dispose ()
 
virtual void update ()=0
 

Protected Attributes

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

Private Member Functions

void initializeROS ()
 

Private Attributes

ros::Publisher markersPub_
 
std::atomic< boolmarkersInitialized_
 
ros::ServiceClient iksrv_
 
std::mutex m_mutex_
 
std::function< void(moveit_msgs::RobotTrajectory &)> postJointDiscontinuityEvent
 
std::function< void(moveit_msgs::RobotTrajectory &)> postIncorrectInitialStateEvent
 
std::function< void()> postMotionExecutionFailureEvents
 
bool autocleanmarkers = false
 

Detailed Description

Definition at line 35 of file cb_move_end_effector_trajectory.h.

Constructor & Destructor Documentation

◆ CbMoveEndEffectorTrajectory() [1/2]

cl_move_group_interface::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( std::string  tipLink = "")

◆ CbMoveEndEffectorTrajectory() [2/2]

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

Definition at line 22 of file cb_move_end_effector_trajectory.cpp.

23 : endEffectorTrajectory_(endEffectorTrajectory), tipLink_(tipLink), markersInitialized_(false)
24
25 {
27 }
std::vector< geometry_msgs::PoseStamped > endEffectorTrajectory_

References initializeROS().

Here is the call graph for this function:

Member Function Documentation

◆ computeJointSpaceTrajectory()

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

Definition at line 36 of file cb_move_end_effector_trajectory.cpp.

37 {
38 // get current robot state
40
41 // get the IK client
42 auto groupname = movegroupClient_->moveGroupClientInterface.getName();
43 auto currentjointnames = currentState->getJointModelGroup(groupname)->getActiveJointModelNames();
44
45 if (!tipLink_ || *tipLink_ == "")
46 {
48 }
49
50 std::vector<double> jointPositions;
51 currentState->copyJointGroupPositions(groupname, jointPositions);
52
53 std::vector<std::vector<double>> trajectory;
54 std::vector<ros::Duration> trajectoryTimeStamps;
55
56 trajectory.push_back(jointPositions);
57 trajectoryTimeStamps.push_back(ros::Duration(0));
58
59 auto &first = endEffectorTrajectory_.front();
60 ros::Time referenceTime = first.header.stamp;
61
62 std::vector<int> discontinuityIndexes;
63
64 int ikAttempts = 4;
65 for (int k = 0; k < this->endEffectorTrajectory_.size(); k++)
66 {
67 auto &pose = this->endEffectorTrajectory_[k];
68 moveit_msgs::GetPositionIKRequest req;
69 // req.ik_request.attempts = 20;
70
71 req.ik_request.ik_link_name = *tipLink_;
72 req.ik_request.robot_state.joint_state.name = currentjointnames;
73 req.ik_request.robot_state.joint_state.position = jointPositions;
74
75 req.ik_request.group_name = groupname;
76 req.ik_request.avoid_collisions = true;
77
78 moveit_msgs::GetPositionIKResponse res;
79
80 //pose.header.stamp = ros::Time::now();
81 req.ik_request.pose_stamped = pose;
82
83 ROS_DEBUG_STREAM("IK request: " << req);
84 if (iksrv_.call(req, res))
85 {
86 auto &prevtrajpoint = trajectory.back();
87 //jointPositions.clear();
88
89 std::stringstream ss;
90 for (int j = 0; j < res.solution.joint_state.position.size(); j++)
91 {
92 auto &jointname = res.solution.joint_state.name[j];
93 auto it = std::find(currentjointnames.begin(), currentjointnames.end(), jointname);
94 if (it != currentjointnames.end())
95 {
96 int index = std::distance(currentjointnames.begin(), it);
97 jointPositions[index] = res.solution.joint_state.position[j];
98 ss << jointname << "(" << index << "): " << jointPositions[index] << std::endl;
99 }
100 }
101
102 // continuity check
103 int jointindex = 0;
104 int discontinuityJointIndex = -1;
105 double discontinuityDeltaJointIndex = -1;
106 double deltajoint;
107
108 bool check = k > 0 || !allowInitialTrajectoryStateJointDiscontinuity_ || allowInitialTrajectoryStateJointDiscontinuity_ && !(*allowInitialTrajectoryStateJointDiscontinuity_);
109 if (check)
110 {
111 for (jointindex = 0; jointindex < jointPositions.size(); jointindex++)
112 {
113 deltajoint = jointPositions[jointindex] - prevtrajpoint[jointindex];
114
115 if (fabs(deltajoint) > 0.3 /*2.5 deg*/)
116 {
117 discontinuityDeltaJointIndex = deltajoint;
118 discontinuityJointIndex = jointindex;
119 }
120 }
121 }
122
123 if (ikAttempts > 0 && discontinuityJointIndex != -1)
124 {
125 k--;
126 ikAttempts--;
127 continue;
128 }
129 else
130 {
131 bool discontinuity = false;
132 if (ikAttempts == 0)
133 {
134 discontinuityIndexes.push_back(k);
135 discontinuity = true;
136 }
137
138 ikAttempts = 4;
139
140 if (discontinuity && discontinuityJointIndex != -1)
141 {
142 // show a message and stop the trajectory generation && jointindex!= 7 || fabs(deltajoint) > 0.1 /*2.5 deg*/ && jointindex== 7
143 std::stringstream ss;
144 ss << "Traj[" << k << "/" << endEffectorTrajectory_.size() << "] " << currentjointnames[discontinuityJointIndex] << " IK discontinuity : " << discontinuityDeltaJointIndex << std::endl
145 << "prev joint value: " << prevtrajpoint[discontinuityJointIndex] << std::endl
146 << "current joint value: " << jointPositions[discontinuityJointIndex] << std::endl;
147
148 ss << std::endl;
149 for (int ji = 0; ji < jointPositions.size(); ji++)
150 {
151 ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl;
152 }
153
154 for (int kindex = 0; kindex < trajectory.size(); kindex++)
155 {
156 ss << "[" << kindex << "]: " << trajectory[kindex][discontinuityJointIndex] << std::endl;
157 }
158
159 if (k == 0)
160 {
161 ss << "This is the first posture of the trajectory. Maybe the robot initial posture is not coincident to the initial posture of the generated joint trajectory." << std::endl;
162 }
163
164 ROS_ERROR_STREAM(ss.str());
165
166 trajectory.push_back(jointPositions);
167 ros::Duration durationFromStart = pose.header.stamp - referenceTime;
168 trajectoryTimeStamps.push_back(durationFromStart);
169
170 continue;
171 }
172 else
173 {
174 trajectory.push_back(jointPositions);
175 ros::Duration durationFromStart = pose.header.stamp - referenceTime;
176 trajectoryTimeStamps.push_back(durationFromStart);
177
178 ROS_DEBUG_STREAM("IK solution: " << res.solution.joint_state);
179 ROS_DEBUG_STREAM("trajpoint: " << std::endl
180 << ss.str());
181 }
182 }
183 }
184 else
185 {
186 ROS_ERROR("[CbMoveEndEffectorTrajectory] wrong IK call");
187 }
188
189 ROS_WARN_STREAM("-----");
190 }
191
192 // interpolate speeds?
193
194 // interpolate accelerations?
195
196 // get current robot state
197 // fill plan message
198 // computedMotionPlan.start_state_.joint_state.name = currentjointnames;
199 // computedMotionPlan.start_state_.joint_state.position = trajectory.front();
200 // computedMotionPlan.trajectory_.joint_trajectory.joint_names = currentjointnames;
201
202 computedJointTrajectory.joint_trajectory.joint_names = currentjointnames;
203 int i = 0;
204 for (auto &p : trajectory)
205 {
206 if (i == 0) // not copy the current state in the trajectory (used to solve discontinuity in other behaviors)
207 {
208 i++;
209 continue;
210 }
211
212 trajectory_msgs::JointTrajectoryPoint jp;
213 jp.positions = p;
214 jp.time_from_start = trajectoryTimeStamps[i]; //ros::Duration(t);
215 computedJointTrajectory.joint_trajectory.points.push_back(jp);
216 i++;
217 }
218
219 if (discontinuityIndexes.size())
220 {
221 if (discontinuityIndexes[0] == 0)
223 else
225 }
226
228 }
moveit::planning_interface::MoveGroupInterface moveGroupClientInterface
Definition: cl_movegroup.h:74

References allowInitialTrajectoryStateJointDiscontinuity_, smacc::ISmaccClientBehavior::currentState, endEffectorTrajectory_, iksrv_, cl_move_group_interface::INCORRECT_INITIAL_STATE, cl_move_group_interface::JOINT_TRAJECTORY_DISCONTINUITY, movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::SUCCESS, and tipLink_.

Referenced by onEntry().

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 337 of file cb_move_end_effector_trajectory.cpp.

338 {
339 tf::Transform localdirection;
340 localdirection.setIdentity();
341 localdirection.setOrigin(tf::Vector3(0.05, 0, 0));
342 auto frameid = this->endEffectorTrajectory_.front().header.frame_id;
343
344 for (auto &pose : this->endEffectorTrajectory_)
345 {
346 visualization_msgs::Marker marker;
347 marker.header.frame_id = frameid;
348 marker.header.stamp = ros::Time::now();
349 marker.ns = "trajectory";
350 marker.id = this->beahiorMarkers_.markers.size();
351 marker.type = visualization_msgs::Marker::ARROW;
352 marker.action = visualization_msgs::Marker::ADD;
353 marker.scale.x = 0.005;
354 marker.scale.y = 0.01;
355 marker.scale.z = 0.01;
356 marker.color.a = 0.8;
357 marker.color.r = 1.0;
358 marker.color.g = 0;
359 marker.color.b = 0;
360
361 geometry_msgs::Point start, end;
362 start.x = 0;
363 start.y = 0;
364 start.z = 0;
365
366 tf::Transform basetransform;
367 tf::poseMsgToTF(pose.pose, basetransform);
368 tf::Transform endarrow = localdirection * basetransform;
369
370 end.x = localdirection.getOrigin().x();
371 end.y = localdirection.getOrigin().y();
372 end.z = localdirection.getOrigin().z();
373
374 marker.pose.position = pose.pose.position;
375 marker.pose.orientation = pose.pose.orientation;
376 marker.points.push_back(start);
377 marker.points.push_back(end);
378
379 beahiorMarkers_.markers.push_back(marker);
380 }
381 }

References beahiorMarkers_, and endEffectorTrajectory_.

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

Here is the caller graph for this function:

◆ executeJointSpaceTrajectory()

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

Definition at line 230 of file cb_move_end_effector_trajectory.cpp.

231 {
232 ROS_INFO_STREAM("[" << this->getName() << "] Executing joint trajectory");
233 // call execute
234 auto executionResult = this->movegroupClient_->moveGroupClientInterface.execute(computedJointTrajectory);
235
236 if (executionResult == moveit_msgs::MoveItErrorCodes::SUCCESS)
237 {
238 ROS_INFO_STREAM("[" << this->getName() << "] motion execution succeeded");
240 this->postSuccessEvent();
241 }
242 else
243 {
245 this->postFailureEvent();
246 }
247 }

References smacc::ISmaccClientBehavior::getName(), movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, and smacc::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 ( )
protectedvirtual

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

Definition at line 383 of file cb_move_end_effector_trajectory.cpp.

384 {
385 // bypass current trajectory, overridden in derived classes
386 // this->endEffectorTrajectory_ = ...
387 }

Referenced by onEntry().

Here is the caller graph for this function:

◆ getCurrentEndEffectorPose()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose ( std::string  globalFrame,
tf::StampedTransform &  currentEndEffectorTransform 
)
protected

Definition at line 389 of file cb_move_end_effector_trajectory.cpp.

390 {
391 tf::TransformListener tfListener;
392
393 try
394 {
395 if (!tipLink_ || *tipLink_ == "")
396 {
397 tipLink_ = this->movegroupClient_->moveGroupClientInterface.getEndEffectorLink();
398 }
399
400 tfListener.waitForTransform(globalFrame, *tipLink_, ros::Time(0), ros::Duration(10));
401 tfListener.lookupTransform(globalFrame, *tipLink_, ros::Time(0), currentEndEffectorTransform);
402
403 // we define here the global frame as the pivot frame id
404 // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, ros::Time(0), ros::Duration(10));
405 // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, ros::Time(0), globalBaseLink);
406 }
407 catch (const std::exception &e)
408 {
409 std::cerr << e.what() << '\n';
410 }
411 }

References movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, 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 caller graph for this function:

◆ initializeROS()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::initializeROS ( )
private

Definition at line 29 of file cb_move_end_effector_trajectory.cpp.

30 {
31 ros::NodeHandle nh;
32 markersPub_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory_markers", 1);
33 iksrv_ = nh.serviceClient<moveit_msgs::GetPositionIK>("/compute_ik");
34 }

References iksrv_, and markersPub_.

Referenced by CbMoveEndEffectorTrajectory().

Here is the caller graph for this function:

◆ onEntry()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onEntry ( )
overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

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

Definition at line 249 of file cb_move_end_effector_trajectory.cpp.

250 {
252
253 this->generateTrajectory();
254
255 if (this->endEffectorTrajectory_.size() == 0)
256 {
257 ROS_WARN_STREAM("[" << smacc::demangleSymbol(typeid(*this).name()) << "] No points in the trajectory. Skipping behavior.");
258 return;
259 }
260
261 this->createMarkers();
262 markersInitialized_ = true;
263
264 moveit_msgs::RobotTrajectory computedTrajectory;
265
266 auto errorcode = computeJointSpaceTrajectory(computedTrajectory);
267
268 bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS;
269
270 CpTrajectoryHistory *trajectoryHistory;
271 this->requiresComponent(trajectoryHistory);
272
273 if (!trajectoryGenerationSuccess)
274 {
275 ROS_INFO_STREAM("[" << this->getName() << "] Incorrect trajectory. Posting failure event.");
276 if (trajectoryHistory != nullptr)
277 {
278 moveit_msgs::MoveItErrorCodes error;
279 error.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
280 trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
281 }
282
284 this->postFailureEvent();
285
287 {
288 this->postJointDiscontinuityEvent(computedTrajectory);
289 }
291 {
292 this->postIncorrectInitialStateEvent(computedTrajectory);
293 }
294 return;
295 }
296 else
297 {
298 if (trajectoryHistory != nullptr)
299 {
300 moveit_msgs::MoveItErrorCodes error;
301 error.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
302 trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
303 }
304
305 this->executeJointSpaceTrajectory(computedTrajectory);
306 }
307
308 // handle finishing events
309 } // namespace cl_move_group_interface
std::function< void(moveit_msgs::RobotTrajectory &)> postJointDiscontinuityEvent
void executeJointSpaceTrajectory(const moveit_msgs::RobotTrajectory &computedJointTrajectory)
ComputeJointTrajectoryErrorCode computeJointSpaceTrajectory(moveit_msgs::RobotTrajectory &computedJointTrajectory)
std::function< void(moveit_msgs::RobotTrajectory &)> postIncorrectInitialStateEvent
void requiresComponent(SmaccComponentType *&storage)
void requiresClient(SmaccClientType *&storage)
std::string demangleSymbol()
Definition: introspection.h:75

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

Here is the call graph for this function:

◆ onExit()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onExit ( )
overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

Definition at line 320 of file cb_move_end_effector_trajectory.cpp.

321 {
322 markersInitialized_ = false;
323
325 {
326 std::lock_guard<std::mutex> guard(m_mutex_);
327 for (auto &marker : this->beahiorMarkers_.markers)
328 {
329 marker.header.stamp = ros::Time::now();
330 marker.action = visualization_msgs::Marker::DELETE;
331 }
332
334 }
335 }

References autocleanmarkers, beahiorMarkers_, m_mutex_, markersInitialized_, and markersPub_.

◆ onOrthogonalAllocation()

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

Definition at line 51 of file cb_move_end_effector_trajectory.h.

52 {
53 smacc::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
54
55 postJointDiscontinuityEvent = [this](auto traj) {
56 auto ev = new EvJointDiscontinuity<TSourceObject, TOrthogonal>();
57 ev->trajectory = traj;
58 this->postEvent(ev);
59 };
60
61 postIncorrectInitialStateEvent = [this](auto traj)
62 {
63 auto ev = new EvIncorrectInitialPosition<TSourceObject, TOrthogonal>();
64 ev->trajectory = traj;
65 this->postEvent(ev);
66 };
67
69 {
70 ROS_INFO_STREAM("[" << this->getName() << "] motion execution failed");
72 this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject,TOrthogonal>>();
73 };
74 }

References smacc::ISmaccClientBehavior::getName(), movegroupClient_, smacc::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 smacc::ISmaccUpdatable.

Definition at line 311 of file cb_move_end_effector_trajectory.cpp.

312 {
314 {
315 std::lock_guard<std::mutex> guard(m_mutex_);
317 }
318 }

References beahiorMarkers_, m_mutex_, markersInitialized_, and markersPub_.

Member Data Documentation

◆ allowInitialTrajectoryStateJointDiscontinuity_

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

Definition at line 44 of file cb_move_end_effector_trajectory.h.

Referenced by computeJointSpaceTrajectory().

◆ autocleanmarkers

bool cl_move_group_interface::CbMoveEndEffectorTrajectory::autocleanmarkers = false
private

Definition at line 115 of file cb_move_end_effector_trajectory.h.

Referenced by onExit().

◆ beahiorMarkers_

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

◆ endEffectorTrajectory_

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

◆ group_

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

Definition at line 40 of file cb_move_end_effector_trajectory.h.

◆ iksrv_

ros::ServiceClient cl_move_group_interface::CbMoveEndEffectorTrajectory::iksrv_
private

◆ m_mutex_

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

Definition at line 108 of file cb_move_end_effector_trajectory.h.

Referenced by onExit(), and update().

◆ markersInitialized_

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

Definition at line 104 of file cb_move_end_effector_trajectory.h.

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

◆ markersPub_

ros::Publisher cl_move_group_interface::CbMoveEndEffectorTrajectory::markersPub_
private

Definition at line 102 of file cb_move_end_effector_trajectory.h.

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

◆ movegroupClient_

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

◆ postIncorrectInitialStateEvent

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

Definition at line 111 of file cb_move_end_effector_trajectory.h.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postJointDiscontinuityEvent

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

Definition at line 110 of file cb_move_end_effector_trajectory.h.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postMotionExecutionFailureEvents

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

◆ tipLink_

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

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