interaction_handler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012-2013, Willow Garage, Inc.
5  * Copyright (c) 2013, Ioan A. Sucan
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Adam Leeper */
37 
47 #include <boost/lexical_cast.hpp>
48 #include <boost/math/constants/constants.hpp>
49 #include <algorithm>
50 #include <limits>
51 
52 #include <Eigen/Core>
53 #include <Eigen/Geometry>
54 
55 namespace robot_interaction
56 {
57 InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
58  const robot_state::RobotState& initial_robot_state,
60  : LockedRobotState(initial_robot_state)
61  , name_(fixName(name))
62  , planning_frame_(initial_robot_state.getRobotModel()->getModelFrame())
63  , tf_(tf)
64  , robot_interaction_(NULL)
65  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
66  , display_meshes_(true)
67  , display_controls_(true)
68 {
69  setRobotInteraction(robot_interaction.get());
70 }
71 
72 InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
74  : LockedRobotState(robot_interaction->getRobotModel())
75  , name_(fixName(name))
76  , planning_frame_(robot_interaction->getRobotModel()->getModelFrame())
77  , tf_(tf)
78  , robot_interaction_(NULL)
79  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
80  , display_meshes_(true)
81  , display_controls_(true)
82 {
83  setRobotInteraction(robot_interaction.get());
84 }
85 
86 // DEPRECATED
87 InteractionHandler::InteractionHandler(const std::string& name, const robot_state::RobotState& initial_robot_state,
89  : LockedRobotState(initial_robot_state)
90  , name_(fixName(name))
91  , planning_frame_(initial_robot_state.getRobotModel()->getModelFrame())
92  , tf_(tf)
93  , robot_interaction_(NULL)
95  , display_meshes_(true)
96  , display_controls_(true)
97 {
98 }
99 
100 // DEPRECATED
101 InteractionHandler::InteractionHandler(const std::string& name, const robot_model::RobotModelConstPtr& robot_model,
103  : LockedRobotState(robot_model)
104  , name_(fixName(name))
105  , planning_frame_(robot_model->getModelFrame())
106  , tf_(tf)
107  , robot_interaction_(NULL)
109  , display_meshes_(true)
110  , display_controls_(true)
111 {
112 }
113 
114 std::string InteractionHandler::fixName(std::string name)
115 {
116  std::replace(name.begin(), name.end(), '_', '-'); // we use _ as a special char in marker name
117  return name;
118 }
119 
120 void InteractionHandler::setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::Pose& m)
121 {
122  boost::mutex::scoped_lock slock(offset_map_lock_);
123  offset_map_[eef.eef_group] = m;
124 }
125 
126 void InteractionHandler::setPoseOffset(const JointInteraction& vj, const geometry_msgs::Pose& m)
127 {
128  boost::mutex::scoped_lock slock(offset_map_lock_);
129  offset_map_[vj.joint_name] = m;
130 }
131 
133 {
134  boost::mutex::scoped_lock slock(offset_map_lock_);
135  offset_map_.erase(eef.eef_group);
136 }
137 
139 {
140  boost::mutex::scoped_lock slock(offset_map_lock_);
141  offset_map_.erase(vj.joint_name);
142 }
143 
145 {
146  boost::mutex::scoped_lock slock(offset_map_lock_);
147  offset_map_.clear();
148 }
149 
150 bool InteractionHandler::getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::Pose& m)
151 {
152  boost::mutex::scoped_lock slock(offset_map_lock_);
153  std::map<std::string, geometry_msgs::Pose>::iterator it = offset_map_.find(eef.eef_group);
154  if (it != offset_map_.end())
155  {
156  m = it->second;
157  return true;
158  }
159  return false;
160 }
161 
162 bool InteractionHandler::getPoseOffset(const JointInteraction& vj, geometry_msgs::Pose& m)
163 {
164  boost::mutex::scoped_lock slock(offset_map_lock_);
165  std::map<std::string, geometry_msgs::Pose>::iterator it = offset_map_.find(vj.joint_name);
166  if (it != offset_map_.end())
167  {
168  m = it->second;
169  return true;
170  }
171  return false;
172 }
173 
174 bool InteractionHandler::getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::PoseStamped& ps)
175 {
176  boost::mutex::scoped_lock slock(pose_map_lock_);
177  std::map<std::string, geometry_msgs::PoseStamped>::iterator it = pose_map_.find(eef.eef_group);
178  if (it != pose_map_.end())
179  {
180  ps = it->second;
181  return true;
182  }
183  return false;
184 }
185 
186 bool InteractionHandler::getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::PoseStamped& ps)
187 {
188  boost::mutex::scoped_lock slock(pose_map_lock_);
189  std::map<std::string, geometry_msgs::PoseStamped>::iterator it = pose_map_.find(vj.joint_name);
190  if (it != pose_map_.end())
191  {
192  ps = it->second;
193  return true;
194  }
195  return false;
196 }
197 
199 {
200  boost::mutex::scoped_lock slock(pose_map_lock_);
201  pose_map_.erase(eef.eef_group);
202 }
203 
205 {
206  boost::mutex::scoped_lock slock(pose_map_lock_);
207  pose_map_.erase(vj.joint_name);
208 }
209 
211 {
212  boost::mutex::scoped_lock slock(pose_map_lock_);
213  pose_map_.clear();
214 }
215 
216 void InteractionHandler::setMenuHandler(const std::shared_ptr<interactive_markers::MenuHandler>& mh)
217 {
218  boost::mutex::scoped_lock lock(state_lock_);
219  menu_handler_ = mh;
220 }
221 
222 const std::shared_ptr<interactive_markers::MenuHandler>& InteractionHandler::getMenuHandler()
223 {
224  boost::mutex::scoped_lock lock(state_lock_);
225  return menu_handler_;
226 }
227 
229 {
230  boost::mutex::scoped_lock lock(state_lock_);
231  menu_handler_.reset();
232 }
233 
235  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
236 {
237  if (g.process_feedback)
238  {
239  StateChangeCallbackFn callback;
240  // modify the RobotState in-place with the state_lock_ held.
242  boost::bind(&InteractionHandler::updateStateGeneric, this, _1, &g, &feedback, &callback));
243 
244  // This calls update_callback_ to notify client that state changed.
245  if (callback)
246  callback(this);
247  }
248 }
249 
251  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
252 {
253  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
254  return;
255 
256  geometry_msgs::PoseStamped tpose;
257  geometry_msgs::Pose offset;
258  if (!getPoseOffset(eef, offset))
259  offset.orientation.w = 1;
260  if (transformFeedbackPose(feedback, offset, tpose))
261  {
262  pose_map_lock_.lock();
263  pose_map_[eef.eef_group] = tpose;
264  pose_map_lock_.unlock();
265  }
266  else
267  return;
268 
269  StateChangeCallbackFn callback;
270 
271  // modify the RobotState in-place with state_lock_ held.
272  // This locks state_lock_ before calling updateState()
274  boost::bind(&InteractionHandler::updateStateEndEffector, this, _1, &eef, &tpose.pose, &callback));
275 
276  // This calls update_callback_ to notify client that state changed.
277  if (callback)
278  callback(this);
279 }
280 
282  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
283 {
284  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
285  return;
286 
287  geometry_msgs::PoseStamped tpose;
288  geometry_msgs::Pose offset;
289  if (!getPoseOffset(vj, offset))
290  offset.orientation.w = 1;
291  if (transformFeedbackPose(feedback, offset, tpose))
292  {
293  pose_map_lock_.lock();
294  pose_map_[vj.joint_name] = tpose;
295  pose_map_lock_.unlock();
296  }
297  else
298  return;
299 
300  StateChangeCallbackFn callback;
301 
302  // modify the RobotState in-place with state_lock_ held.
303  // This locks state_lock_ before calling updateState()
305  boost::bind(&InteractionHandler::updateStateJoint, this, _1, &vj, &tpose.pose, &callback));
306 
307  // This calls update_callback_ to notify client that state changed.
308  if (callback)
309  callback(this);
310 }
311 
312 // MUST hold state_lock_ when calling this!
313 void InteractionHandler::updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g,
314  const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback,
315  StateChangeCallbackFn* callback)
316 {
317  bool ok = g->process_feedback(*state, *feedback);
318  bool error_state_changed = setErrorState(g->marker_name_suffix, !ok);
319  if (update_callback_)
320  *callback = boost::bind(update_callback_, _1, error_state_changed);
321 }
322 
323 // MUST hold state_lock_ when calling this!
324 void InteractionHandler::updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef,
325  const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback)
326 {
327  // This is called with state_lock_ held, so no additional locking needed to
328  // access kinematic_options_map_.
329  KinematicOptions kinematic_options = kinematic_options_map_->getOptions(eef->parent_group);
330 
331  bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose);
332  bool error_state_changed = setErrorState(eef->parent_group, !ok);
333  if (update_callback_)
334  *callback = boost::bind(update_callback_, _1, error_state_changed);
335 }
336 
337 // MUST hold state_lock_ when calling this!
338 void InteractionHandler::updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj,
339  const geometry_msgs::Pose* feedback_pose, StateChangeCallbackFn* callback)
340 {
341  geometry_msgs::Pose rel_pose = *feedback_pose;
342  if (!vj->parent_frame.empty() && !robot_state::Transforms::sameFrame(vj->parent_frame, planning_frame_))
343  {
344  Eigen::Affine3d p;
345  tf::poseMsgToEigen(rel_pose, p);
346  tf::poseEigenToMsg(state->getGlobalLinkTransform(vj->parent_frame).inverse() * p, rel_pose);
347  }
348 
349  Eigen::Quaterniond q;
350  tf::quaternionMsgToEigen(rel_pose.orientation, q);
351  std::map<std::string, double> vals;
352  if (vj->dof == 3)
353  {
354  vals[vj->joint_name + "/x"] = rel_pose.position.x;
355  vals[vj->joint_name + "/y"] = rel_pose.position.y;
356  Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);
357  vals[vj->joint_name + "/theta"] = xyz[2];
358  }
359  else if (vj->dof == 6)
360  {
361  vals[vj->joint_name + "/trans_x"] = rel_pose.position.x;
362  vals[vj->joint_name + "/trans_y"] = rel_pose.position.y;
363  vals[vj->joint_name + "/trans_z"] = rel_pose.position.z;
364  vals[vj->joint_name + "/rot_x"] = q.x();
365  vals[vj->joint_name + "/rot_y"] = q.y();
366  vals[vj->joint_name + "/rot_z"] = q.z();
367  vals[vj->joint_name + "/rot_w"] = q.w();
368  }
369  state->setVariablePositions(vals);
370  state->update();
371 
372  if (update_callback_)
373  *callback = boost::bind(update_callback_, _1, false);
374 }
375 
377 {
378  return getErrorState(eef.parent_group);
379 }
380 
382 {
384 }
385 
387 {
388  return false;
389 }
390 
392 {
393  boost::mutex::scoped_lock lock(state_lock_);
394  error_state_.clear();
395 }
396 
397 // return true if the state changed.
398 // MUST hold state_lock_ when calling this!
399 bool InteractionHandler::setErrorState(const std::string& name, bool new_error_state)
400 {
401  bool old_error_state = error_state_.find(name) != error_state_.end();
402 
403  if (new_error_state == old_error_state)
404  return false;
405 
406  if (new_error_state)
407  error_state_.insert(name);
408  else
409  error_state_.erase(name);
410 
411  return true;
412 }
413 
414 bool InteractionHandler::getErrorState(const std::string& name) const
415 {
416  boost::mutex::scoped_lock lock(state_lock_);
417  return error_state_.find(name) != error_state_.end();
418 }
419 
420 bool InteractionHandler::transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
421  const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose)
422 {
423  tpose.header = feedback->header;
424  tpose.pose = feedback->pose;
425  if (feedback->header.frame_id != planning_frame_)
426  {
427  if (tf_)
428  try
429  {
430  tf::Stamped<tf::Pose> spose;
431  tf::poseStampedMsgToTF(tpose, spose);
432  // Express feedback (marker) pose in planning frame
433  tf_->transformPose(planning_frame_, spose, spose);
434  // Apply inverse of offset to bring feedback pose back into the end-effector support link frame
435  tf::Transform tf_offset;
436  tf::poseMsgToTF(offset, tf_offset);
437  spose.setData(spose * tf_offset.inverse());
438  tf::poseStampedTFToMsg(spose, tpose);
439  }
440  catch (tf::TransformException& e)
441  {
442  ROS_ERROR("Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(),
443  planning_frame_.c_str());
444  return false;
445  }
446  else
447  {
448  ROS_ERROR("Cannot transform from frame '%s' to frame '%s' (no TF instance provided)",
449  tpose.header.frame_id.c_str(), planning_frame_.c_str());
450  return false;
451  }
452  }
453  return true;
454 }
455 
456 // This syncs the InteractionHandler's KinematicOptionsMap with the
457 // RobotInteraction's. After this both will share the same
458 // KinematicOptionsMap.
459 //
460 // With the constructors that take a RobotInteraction parameter this function
461 // is not needed (except as a sanity check that the RobotInteraction and
462 // InteractionHandler are matched). This function is necessary because when
463 // the old constructors are used the InteractionHandler may not know what
464 // RobotInteraction it is associated with until after some options have been
465 // set on the InteractionHandler.
467 {
468  boost::mutex::scoped_lock lock(state_lock_);
469 
470  // Verivy that this InteractionHandler is only used with one
471  // RobotInteraction.
472  // This is the only use for robot_interaction_.
473  if (robot_interaction_)
474  {
475  if (robot_interaction_ != robot_interaction)
476  {
477  ROS_ERROR("setKinematicOptions() called from 2 different RobotInteraction instances.");
478  }
479  return;
480  }
481 
482  robot_interaction_ = robot_interaction;
483 
484  KinematicOptionsMapPtr shared_kinematic_options_map = robot_interaction->getKinematicOptionsMap();
485 
486  // merge old options into shared options
487  // This is necessary because some legacy code sets values using deprecated
488  // InteractionHandler methods before a RobotInteraction is associated with
489  // this InteractionHandler.
490  //
491  // This is a nop if a constructor with a robot_interaction parameter is used.
492  shared_kinematic_options_map->merge(*kinematic_options_map_);
493 
494  // from now on the InteractionHandler shares the same KinematicOptionsMap
495  // with RobotInteraction.
496  // The old *kinematic_options_map_ is automatically deleted by boost::shared_ptr.
497  //
498  // This is a nop if a constructor with a robot_interaction parameter is used.
499  kinematic_options_map_ = shared_kinematic_options_map;
500 }
501 
502 // ============= DEPRECATED FUNCTIONS =====================
503 
505 {
506  KinematicOptions delta;
507  delta.timeout_seconds_ = timeout;
508 
509  boost::mutex::scoped_lock lock(state_lock_);
511 }
512 
513 void InteractionHandler::setIKAttempts(unsigned int attempts)
514 {
515  KinematicOptions delta;
516  delta.max_attempts_ = attempts;
517 
518  boost::mutex::scoped_lock lock(state_lock_);
520 }
521 
523 {
524  KinematicOptions delta;
525  delta.options_ = opt;
526 
527  boost::mutex::scoped_lock lock(state_lock_);
529 }
530 
531 void InteractionHandler::setKinematicsQueryOptionsForGroup(const std::string& group_name,
533 {
534  KinematicOptions delta;
535  delta.options_ = opt;
536 
537  boost::mutex::scoped_lock lock(state_lock_);
538  kinematic_options_map_->setOptions(group_name, delta, KinematicOptions::ALL_QUERY_OPTIONS);
539 }
540 
541 void InteractionHandler::setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn& callback)
542 {
543  KinematicOptions delta;
544  delta.state_validity_callback_ = callback;
545 
546  boost::mutex::scoped_lock lock(state_lock_);
548 }
549 
551 {
552  boost::mutex::scoped_lock lock(state_lock_);
553  return kinematic_options_map_->getOptions(KinematicOptionsMap::DEFAULT).options_;
554 }
555 
557 {
558  boost::mutex::scoped_lock lock(state_lock_);
559  update_callback_ = callback;
560 }
561 
563 {
564  boost::mutex::scoped_lock lock(state_lock_);
565  return update_callback_;
566 }
567 
569 {
570  boost::mutex::scoped_lock lock(state_lock_);
571  display_meshes_ = visible;
572 }
573 
575 {
576  boost::mutex::scoped_lock lock(state_lock_);
577  return display_meshes_;
578 }
579 
581 {
582  boost::mutex::scoped_lock lock(state_lock_);
583  display_controls_ = visible;
584 }
585 
587 {
588  boost::mutex::scoped_lock lock(state_lock_);
589  return display_controls_;
590 }
591 }
void clearPoseOffsets()
Clear the pose offset for all end-effectors and virtual joints.
void updateStateJoint(robot_state::RobotState *state, const JointInteraction *vj, const geometry_msgs::Pose *pose, StateChangeCallbackFn *callback)
robot_state::GroupStateValidityCallbackFn state_validity_callback_
This is called to determine if the state is valid.
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
std::string parent_link
The name of the link in the parent group that connects to the end-effector.
Definition: interaction.h:147
boost::function< void(InteractionHandler *handler, bool error_state_changed)> update_callback_
bool getErrorState(const std::string &name) const
void setData(const T &input)
void clearLastMarkerPoses()
Clear the last interactive_marker command poses for all end-effectors and joints. ...
static const std::string DEFAULT
When used as key this means the default value.
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
void clearMenuHandler()
Remove the menu handler for this interaction handler.
kinematics::KinematicsQueryOptions getKinematicsQueryOptions() const
void setRobotInteraction(RobotInteraction *robot_interaction)
This should only be called by RobotInteraction. Associates this InteractionHandler to a RobotInteract...
KinematicOptionsMapPtr getKinematicOptionsMap()
void clearLastJointMarkerPose(const JointInteraction &vj)
Clear the last interactive_marker command pose for the given joint.
double timeout_seconds_
max time an IK attempt can take before we give up.
std::map< std::string, geometry_msgs::Pose > offset_map_
bool getLastJointMarkerPose(const JointInteraction &vj, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for a joint.
void setKinematicsQueryOptionsForGroup(const std::string &group_name, const kinematics::KinematicsQueryOptions &options)
bool getLastEndEffectorMarkerPose(const EndEffectorInteraction &eef, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for an end-effector.
void setPoseOffset(const EndEffectorInteraction &eef, const geometry_msgs::Pose &m)
Set the offset from EEF to its marker.
virtual void handleJoint(const JointInteraction &vj, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
virtual void handleEndEffector(const EndEffectorInteraction &eef, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
void clearPoseOffset(const EndEffectorInteraction &eef)
Clear the interactive marker pose offset for the given end-effector.
std::string eef_group
The name of the group that defines the group joints.
Definition: interaction.h:150
bool getPoseOffset(const EndEffectorInteraction &eef, geometry_msgs::Pose &m)
Get the offset from EEF to its marker.
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_
static std::string fixName(std::string name)
boost::function< void(InteractionHandler *)> StateChangeCallbackFn
boost::shared_ptr< tf::Transformer > tf_
const std::shared_ptr< interactive_markers::MenuHandler > & getMenuHandler()
Get the menu handler that defines menus and callbacks for all interactive markers drawn by this inter...
kinematics::KinematicsQueryOptions options_
other options
virtual bool inError(const EndEffectorInteraction &eef) const
Check if the marker corresponding to this end-effector leads to an invalid state. ...
std::string parent_group
The name of the group that sustains the end-effector (usually an arm)
Definition: interaction.h:144
void updateStateGeneric(robot_state::RobotState *state, const GenericInteraction *g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr *feedback, StateChangeCallbackFn *callback)
unsigned int max_attempts_
how many attempts before we give up.
Transform inverse() const
boost::function< void(InteractionHandler *, bool)> InteractionHandlerCallbackFn
void modifyState(const ModifyStateFunction &modify)
void clearLastEndEffectorMarkerPose(const EndEffectorInteraction &eef)
Clear the last interactive_marker command pose for the given end-effector.
bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const geometry_msgs::Pose &offset, geometry_msgs::PoseStamped &tpose)
void setKinematicsQueryOptions(const kinematics::KinematicsQueryOptions &opt)
void updateStateEndEffector(robot_state::RobotState *state, const EndEffectorInteraction *eef, const geometry_msgs::Pose *pose, StateChangeCallbackFn *callback)
const InteractionHandlerCallbackFn & getUpdateCallback() const
std::map< std::string, geometry_msgs::PoseStamped > pose_map_
std::string parent_frame
The name of the frame that is a parent of this joint.
Definition: interaction.h:167
unsigned int dof
The type of joint disguised as the number of DOF it has. 3=PLANAR in X/Y; 6=FLOATING.
Definition: interaction.h:173
void setUpdateCallback(const InteractionHandlerCallbackFn &callback)
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
static const std::string ALL
When used as key this means set ALL keys (including default)
void clearError(void)
Clear any error settings. This makes the markers appear as if the state is no longer invalid...
Maintain a RobotState in a multithreaded environment.
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
virtual void handleGeneric(const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Update the internal state maintained by the handler using information from the received feedback mess...
std::string joint_name
The name of the joint.
Definition: interaction.h:170
InteractionHandler(const RobotInteractionPtr &robot_interaction, const std::string &name, const robot_state::RobotState &initial_robot_state, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >())
void setIKAttempts(unsigned int attempts)
#define ROS_ERROR(...)
bool setStateFromIK(robot_state::RobotState &state, const std::string &group, const std::string &tip, const geometry_msgs::Pose &pose) const
void setMenuHandler(const std::shared_ptr< interactive_markers::MenuHandler > &mh)
Set the menu handler that defines menus and callbacks for all interactive markers drawn by this inter...
bool setErrorState(const std::string &name, bool new_error_state)
void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn &callback)


robot_interaction
Author(s): Ioan Sucan
autogenerated on Sun Jan 21 2018 03:55:57