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 
46 #include <tf2_eigen/tf2_eigen.h>
48 #include <boost/lexical_cast.hpp>
49 #include <boost/math/constants/constants.hpp>
50 #include <algorithm>
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 moveit::core::RobotState& initial_robot_state,
59  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
60  : LockedRobotState(initial_robot_state)
61  , name_(fixName(name))
62  , planning_frame_(robot_interaction->getRobotModel()->getModelFrame())
63  , tf_buffer_(tf_buffer)
64  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
65  , display_meshes_(true)
66  , display_controls_(true)
67 {
68 }
69 
70 InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
71  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
72  : LockedRobotState(robot_interaction->getRobotModel())
73  , name_(fixName(name))
74  , planning_frame_(robot_interaction->getRobotModel()->getModelFrame())
75  , tf_buffer_(tf_buffer)
76  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
77  , display_meshes_(true)
78  , display_controls_(true)
79 {
80 }
81 
82 std::string InteractionHandler::fixName(std::string name)
83 {
84  std::replace(name.begin(), name.end(), '_', '-'); // we use _ as a special char in marker name
85  return name;
86 }
87 
88 void InteractionHandler::setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::Pose& m)
89 {
90  boost::mutex::scoped_lock slock(offset_map_lock_);
91  offset_map_[eef.eef_group] = m;
92 }
93 
94 void InteractionHandler::setPoseOffset(const JointInteraction& vj, const geometry_msgs::Pose& m)
95 {
96  boost::mutex::scoped_lock slock(offset_map_lock_);
97  offset_map_[vj.joint_name] = m;
98 }
99 
100 void InteractionHandler::clearPoseOffset(const EndEffectorInteraction& eef)
101 {
102  boost::mutex::scoped_lock slock(offset_map_lock_);
103  offset_map_.erase(eef.eef_group);
104 }
105 
107 {
108  boost::mutex::scoped_lock slock(offset_map_lock_);
109  offset_map_.erase(vj.joint_name);
110 }
111 
113 {
114  boost::mutex::scoped_lock slock(offset_map_lock_);
115  offset_map_.clear();
116 }
117 
118 bool InteractionHandler::getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::Pose& m)
119 {
120  boost::mutex::scoped_lock slock(offset_map_lock_);
121  std::map<std::string, geometry_msgs::Pose>::iterator it = offset_map_.find(eef.eef_group);
122  if (it != offset_map_.end())
123  {
124  m = it->second;
125  return true;
126  }
127  return false;
128 }
129 
130 bool InteractionHandler::getPoseOffset(const JointInteraction& vj, geometry_msgs::Pose& m)
131 {
132  boost::mutex::scoped_lock slock(offset_map_lock_);
133  std::map<std::string, geometry_msgs::Pose>::iterator it = offset_map_.find(vj.joint_name);
134  if (it != offset_map_.end())
135  {
136  m = it->second;
137  return true;
138  }
139  return false;
140 }
141 
142 bool InteractionHandler::getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::PoseStamped& ps)
143 {
144  boost::mutex::scoped_lock slock(pose_map_lock_);
145  std::map<std::string, geometry_msgs::PoseStamped>::iterator it = pose_map_.find(eef.eef_group);
146  if (it != pose_map_.end())
147  {
148  ps = it->second;
149  return true;
150  }
151  return false;
152 }
153 
154 bool InteractionHandler::getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::PoseStamped& ps)
155 {
156  boost::mutex::scoped_lock slock(pose_map_lock_);
157  std::map<std::string, geometry_msgs::PoseStamped>::iterator it = pose_map_.find(vj.joint_name);
158  if (it != pose_map_.end())
159  {
160  ps = it->second;
161  return true;
162  }
163  return false;
164 }
165 
167 {
168  boost::mutex::scoped_lock slock(pose_map_lock_);
169  pose_map_.erase(eef.eef_group);
170 }
171 
172 void InteractionHandler::clearLastJointMarkerPose(const JointInteraction& vj)
173 {
174  boost::mutex::scoped_lock slock(pose_map_lock_);
175  pose_map_.erase(vj.joint_name);
176 }
177 
179 {
180  boost::mutex::scoped_lock slock(pose_map_lock_);
181  pose_map_.clear();
182 }
183 
184 void InteractionHandler::setMenuHandler(const std::shared_ptr<interactive_markers::MenuHandler>& mh)
185 {
186  boost::mutex::scoped_lock lock(state_lock_);
188 }
189 
190 const std::shared_ptr<interactive_markers::MenuHandler>& InteractionHandler::getMenuHandler()
191 {
192  boost::mutex::scoped_lock lock(state_lock_);
193  return menu_handler_;
194 }
195 
197 {
198  boost::mutex::scoped_lock lock(state_lock_);
199  menu_handler_.reset();
200 }
201 
203  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
204 {
206  {
207  StateChangeCallbackFn callback;
208  // modify the RobotState in-place with the state_lock_ held.
209  LockedRobotState::modifyState([this, &g, &feedback, &callback](moveit::core::RobotState* state) {
210  updateStateGeneric(*state, g, feedback, callback);
211  });
212 
213  // This calls update_callback_ to notify client that state changed.
214  if (callback)
215  callback(this);
216  }
217 }
218 
220  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
221 {
222  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
223  return;
224 
225  geometry_msgs::PoseStamped tpose;
226  geometry_msgs::Pose offset;
227  if (!getPoseOffset(eef, offset))
228  offset.orientation.w = 1;
229  if (transformFeedbackPose(feedback, offset, tpose))
230  {
231  pose_map_lock_.lock();
232  pose_map_[eef.eef_group] = tpose;
233  pose_map_lock_.unlock();
234  }
235  else
236  return;
237 
238  StateChangeCallbackFn callback;
239 
240  // modify the RobotState in-place with state_lock_ held.
241  // This locks state_lock_ before calling updateState()
242  LockedRobotState::modifyState([this, &eef, &pose = tpose.pose, &callback](moveit::core::RobotState* state) {
243  updateStateEndEffector(*state, eef, pose, callback);
244  });
245 
246  // This calls update_callback_ to notify client that state changed.
247  if (callback)
248  callback(this);
249 }
250 
252  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
253 {
254  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
255  return;
256 
257  geometry_msgs::PoseStamped tpose;
258  geometry_msgs::Pose offset;
259  if (!getPoseOffset(vj, offset))
260  offset.orientation.w = 1;
261  if (transformFeedbackPose(feedback, offset, tpose))
262  {
263  pose_map_lock_.lock();
264  pose_map_[vj.joint_name] = tpose;
265  pose_map_lock_.unlock();
266  }
267  else
268  return;
269 
270  StateChangeCallbackFn callback;
271 
272  // modify the RobotState in-place with state_lock_ held.
273  // This locks state_lock_ before calling updateState()
274  LockedRobotState::modifyState([this, &vj, &pose = tpose.pose, &callback](moveit::core::RobotState* state) {
275  updateStateJoint(*state, vj, pose, callback);
276  });
277 
278  // This calls update_callback_ to notify client that state changed.
279  if (callback)
280  callback(this);
281 }
282 
283 // MUST hold state_lock_ when calling this!
285  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
286  StateChangeCallbackFn& callback)
287 {
288  bool ok = g.process_feedback(state, feedback);
289  bool error_state_changed = setErrorState(g.marker_name_suffix, !ok);
290  if (update_callback_)
291  callback = [cb = this->update_callback_, error_state_changed](robot_interaction::InteractionHandler* handler) {
292  cb(handler, error_state_changed);
293  };
294 }
295 
296 // MUST hold state_lock_ when calling this!
298  const geometry_msgs::Pose& pose, StateChangeCallbackFn& callback)
299 {
300  // This is called with state_lock_ held, so no additional locking needed to
301  // access kinematic_options_map_.
302  KinematicOptions kinematic_options = kinematic_options_map_->getOptions(eef.parent_group);
303 
304  bool ok = kinematic_options.setStateFromIK(state, eef.parent_group, eef.parent_link, pose);
305  bool error_state_changed = setErrorState(eef.parent_group, !ok);
306  if (update_callback_)
307  callback = [cb = this->update_callback_, error_state_changed](robot_interaction::InteractionHandler* handler) {
308  cb(handler, error_state_changed);
309  };
310 }
311 
312 // MUST hold state_lock_ when calling this!
313 void InteractionHandler::updateStateJoint(moveit::core::RobotState& state, const JointInteraction& vj,
314  const geometry_msgs::Pose& feedback_pose, StateChangeCallbackFn& callback)
315 {
316  Eigen::Isometry3d pose;
317  tf2::fromMsg(feedback_pose, pose);
318 
319  if (!vj.parent_frame.empty() && !moveit::core::Transforms::sameFrame(vj.parent_frame, planning_frame_))
320  pose = state.getGlobalLinkTransform(vj.parent_frame).inverse() * pose;
321 
322  state.setJointPositions(vj.joint_name, pose);
323  state.update();
324 
325  if (update_callback_)
326  callback = [cb = this->update_callback_](robot_interaction::InteractionHandler* handler) { cb(handler, false); };
327 }
328 
329 bool InteractionHandler::inError(const EndEffectorInteraction& eef) const
330 {
331  return getErrorState(eef.parent_group);
332 }
333 
335 {
337 }
338 
339 bool InteractionHandler::inError(const JointInteraction& /*unused*/) const
340 {
341  return false;
342 }
343 
345 {
346  boost::mutex::scoped_lock lock(state_lock_);
347  error_state_.clear();
348 }
349 
350 // return true if the state changed.
351 // MUST hold state_lock_ when calling this!
352 bool InteractionHandler::setErrorState(const std::string& name, bool new_error_state)
353 {
354  bool old_error_state = error_state_.find(name) != error_state_.end();
355 
356  if (new_error_state == old_error_state)
357  return false;
358 
359  if (new_error_state)
360  error_state_.insert(name);
361  else
362  error_state_.erase(name);
363 
364  return true;
365 }
366 
367 bool InteractionHandler::getErrorState(const std::string& name) const
368 {
369  boost::mutex::scoped_lock lock(state_lock_);
370  return error_state_.find(name) != error_state_.end();
371 }
372 
373 bool InteractionHandler::transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
374  const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose)
375 {
376  tpose.header = feedback->header;
377  tpose.pose = feedback->pose;
378  if (feedback->header.frame_id != planning_frame_)
379  {
380  if (tf_buffer_)
381  try
382  {
383  geometry_msgs::PoseStamped spose(tpose);
384  // Express feedback (marker) pose in planning frame
385  tf_buffer_->transform(tpose, spose, planning_frame_);
386  // Apply inverse of offset to bring feedback pose back into the end-effector support link frame
387  tf2::Transform tf_offset, tf_tpose;
388  tf2::fromMsg(offset, tf_offset);
389  tf2::fromMsg(spose.pose, tf_tpose);
390  tf2::toMsg(tf_tpose * tf_offset.inverse(), tpose.pose);
391  }
392  catch (tf2::TransformException& e)
393  {
394  ROS_ERROR("Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(),
395  planning_frame_.c_str());
396  return false;
397  }
398  else
399  {
400  ROS_ERROR("Cannot transform from frame '%s' to frame '%s' (no TF instance provided)",
401  tpose.header.frame_id.c_str(), planning_frame_.c_str());
402  return false;
403  }
404  }
405  return true;
406 }
407 
409 {
410  boost::mutex::scoped_lock lock(state_lock_);
411  update_callback_ = callback;
412 }
413 
415 {
416  boost::mutex::scoped_lock lock(state_lock_);
417  return update_callback_;
418 }
419 
420 void InteractionHandler::setMeshesVisible(bool visible)
421 {
422  boost::mutex::scoped_lock lock(state_lock_);
423  display_meshes_ = visible;
424 }
425 
427 {
428  boost::mutex::scoped_lock lock(state_lock_);
429  return display_meshes_;
430 }
431 
433 {
434  boost::mutex::scoped_lock lock(state_lock_);
435  display_controls_ = visible;
436 }
437 
439 {
440  boost::mutex::scoped_lock lock(state_lock_);
442 }
443 } // namespace robot_interaction
robot_interaction::InteractionHandler::getMenuHandler
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...
Definition: interaction_handler.cpp:223
robot_interaction::InteractionHandler::menu_handler_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_
Definition: interaction_handler.h:324
robot_interaction::InteractionHandler::getPoseOffset
bool getPoseOffset(const EndEffectorInteraction &eef, geometry_msgs::Pose &m)
Get the offset from EEF to its marker.
Definition: interaction_handler.cpp:151
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
robot_interaction::EndEffectorInteraction::eef_group
std::string eef_group
The name of the group that defines the group joints.
Definition: interaction.h:148
robot_interaction::InteractionHandler::StateChangeCallbackFn
boost::function< void(InteractionHandler *)> StateChangeCallbackFn
Definition: interaction_handler.h:262
tf2::Transform::inverse
Transform inverse() const
robot_interaction::LockedRobotState::state_lock_
boost::mutex state_lock_
Definition: locked_robot_state.h:135
robot_interaction::InteractionHandler::transformFeedbackPose
bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const geometry_msgs::Pose &offset, geometry_msgs::PoseStamped &tpose)
Definition: interaction_handler.cpp:406
robot_interaction::LockedRobotState::modifyState
void modifyState(const ModifyStateFunction &modify)
Definition: locked_robot_state.cpp:79
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
robot_interaction::GenericInteraction
Definition: interaction.h:118
robot_interaction::GenericInteraction::marker_name_suffix
std::string marker_name_suffix
Definition: interaction.h:134
robot_interaction::InteractionHandler::updateStateJoint
void updateStateJoint(moveit::core::RobotState &state, const JointInteraction &vj, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:346
robot_interaction::InteractionHandler::clearLastEndEffectorMarkerPose
void clearLastEndEffectorMarkerPose(const EndEffectorInteraction &eef)
Clear the last interactive_marker command pose for the given end-effector.
Definition: interaction_handler.cpp:199
robot_interaction::InteractionHandler::setUpdateCallback
void setUpdateCallback(const InteractionHandlerCallbackFn &callback)
Definition: interaction_handler.cpp:441
robot_interaction::InteractionHandler::setMeshesVisible
void setMeshesVisible(bool visible)
Definition: interaction_handler.cpp:453
menu_handler.h
robot_interaction::InteractionHandler::pose_map_
std::map< std::string, geometry_msgs::PoseStamped > pose_map_
Definition: interaction_handler.h:305
robot_interaction::InteractionHandlerCallbackFn
boost::function< void(InteractionHandler *, bool)> InteractionHandlerCallbackFn
Definition: interaction_handler.h:87
robot_interaction::JointInteraction
Definition: interaction.h:159
moveit::core::RobotState
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
robot_interaction::EndEffectorInteraction::parent_group
std::string parent_group
The name of the group that sustains the end-effector (usually an arm)
Definition: interaction.h:142
ok
ROSCPP_DECL bool ok()
transforms.h
robot_interaction.h
interaction_handler.h
robot_interaction::InteractionHandler::offset_map_lock_
boost::mutex offset_map_lock_
Definition: interaction_handler.h:308
kinematic_options_map.h
robot_interaction::InteractionHandler::handleGeneric
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...
Definition: interaction_handler.cpp:235
moveit::core::RobotState::update
void update(bool force=false)
name
std::string name
robot_interaction::InteractionHandler::kinematic_options_map_
KinematicOptionsMapPtr kinematic_options_map_
Definition: interaction_handler.h:313
robot_interaction::InteractionHandler::display_controls_
bool display_controls_
Definition: interaction_handler.h:340
robot_interaction::InteractionHandler::clearPoseOffsets
void clearPoseOffsets()
Clear the pose offset for all end-effectors and virtual joints.
Definition: interaction_handler.cpp:145
robot_interaction::InteractionHandler::inError
virtual bool inError(const EndEffectorInteraction &eef) const
Check if the marker corresponding to this end-effector leads to an invalid state.
Definition: interaction_handler.cpp:362
robot_interaction::InteractionHandler::setMenuHandler
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...
Definition: interaction_handler.cpp:217
robot_interaction::EndEffectorInteraction::parent_link
std::string parent_link
The name of the link in the parent group that connects to the end-effector.
Definition: interaction.h:145
robot_interaction::InteractionHandler::setErrorState
bool setErrorState(const std::string &name, bool new_error_state)
Definition: interaction_handler.cpp:385
robot_interaction::InteractionHandler::clearError
void clearError()
Clear any error settings. This makes the markers appear as if the state is no longer invalid.
Definition: interaction_handler.cpp:377
ROS_ERROR
#define ROS_ERROR(...)
tf2::Transform
robot_interaction::JointInteraction::joint_name
std::string joint_name
The name of the joint.
Definition: interaction.h:168
robot_interaction::InteractionHandler::clearLastMarkerPoses
void clearLastMarkerPoses()
Clear the last interactive_marker command poses for all end-effectors and joints.
Definition: interaction_handler.cpp:211
robot_interaction::InteractionHandler::error_state_
std::set< std::string > error_state_
Definition: interaction_handler.h:317
robot_interaction::InteractionHandler::update_callback_
boost::function< void(InteractionHandler *handler, bool error_state_changed)> update_callback_
Definition: interaction_handler.h:334
robot_interaction::InteractionHandler::pose_map_lock_
boost::mutex pose_map_lock_
Definition: interaction_handler.h:307
robot_interaction::EndEffectorInteraction
Definition: interaction.h:139
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
robot_interaction::InteractionHandler::getUpdateCallback
const InteractionHandlerCallbackFn & getUpdateCallback() const
Definition: interaction_handler.cpp:447
robot_interaction::InteractionHandler::setControlsVisible
void setControlsVisible(bool visible)
Definition: interaction_handler.cpp:465
interactive_marker_server.h
robot_interaction::InteractionHandler::display_meshes_
bool display_meshes_
Definition: interaction_handler.h:337
interactive_marker_helpers.h
tf_buffer
tf2_ros::Buffer * tf_buffer
robot_interaction::InteractionHandler::clearMenuHandler
void clearMenuHandler()
Remove the menu handler for this interaction handler.
Definition: interaction_handler.cpp:229
robot_interaction::KinematicOptions
Definition: kinematic_options.h:80
robot_interaction::InteractionHandler::clearLastJointMarkerPose
void clearLastJointMarkerPose(const JointInteraction &vj)
Clear the last interactive_marker command pose for the given joint.
Definition: interaction_handler.cpp:205
robot_interaction::InteractionHandler::getControlsVisible
bool getControlsVisible() const
Definition: interaction_handler.cpp:471
robot_interaction::KinematicOptions::setStateFromIK
bool setStateFromIK(moveit::core::RobotState &state, const std::string &group, const std::string &tip, const geometry_msgs::Pose &pose) const
Definition: kinematic_options.cpp:47
robot_interaction::InteractionHandler::getLastJointMarkerPose
bool getLastJointMarkerPose(const JointInteraction &vj, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for a joint.
Definition: interaction_handler.cpp:187
tf2_geometry_msgs.h
robot_interaction
Definition: interaction.h:54
tf2::toMsg
B toMsg(const A &a)
robot_interaction::InteractionHandler::handleJoint
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...
Definition: interaction_handler.cpp:284
robot_interaction::GenericInteraction::process_feedback
ProcessFeedbackFn process_feedback
Definition: interaction.h:126
robot_interaction::InteractionHandler::planning_frame_
const std::string planning_frame_
Definition: interaction_handler.h:258
robot_interaction::InteractionHandler::getMeshesVisible
bool getMeshesVisible() const
Definition: interaction_handler.cpp:459
robot_interaction::InteractionHandler::clearPoseOffset
void clearPoseOffset(const EndEffectorInteraction &eef)
Clear the interactive marker pose offset for the given end-effector.
Definition: interaction_handler.cpp:133
robot_interaction::InteractionHandler::updateStateGeneric
void updateStateGeneric(moveit::core::RobotState &state, const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:317
robot_interaction::InteractionHandler::offset_map_
std::map< std::string, geometry_msgs::Pose > offset_map_
Definition: interaction_handler.h:296
tf2::TransformException
robot_interaction::InteractionHandler
Definition: interaction_handler.h:108
robot_interaction::InteractionHandler::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: interaction_handler.h:259
robot_interaction::InteractionHandler::getLastEndEffectorMarkerPose
bool getLastEndEffectorMarkerPose(const EndEffectorInteraction &eef, geometry_msgs::PoseStamped &pose)
Get the last interactive_marker command pose for an end-effector.
Definition: interaction_handler.cpp:175
robot_interaction::InteractionHandler::setPoseOffset
void setPoseOffset(const EndEffectorInteraction &eef, const geometry_msgs::Pose &m)
Set the offset from EEF to its marker.
Definition: interaction_handler.cpp:121
robot_interaction::InteractionHandler::InteractionHandler
InteractionHandler(const RobotInteractionPtr &robot_interaction, const std::string &name, const moveit::core::RobotState &initial_robot_state, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >())
Definition: interaction_handler.cpp:90
robot_interaction::InteractionHandler::getErrorState
bool getErrorState(const std::string &name) const
Definition: interaction_handler.cpp:400
robot_interaction::InteractionHandler::handleEndEffector
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...
Definition: interaction_handler.cpp:252
robot_interaction::InteractionHandler::updateStateEndEffector
void updateStateEndEffector(moveit::core::RobotState &state, const EndEffectorInteraction &eef, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback)
Definition: interaction_handler.cpp:330
robot_interaction::InteractionHandler::fixName
static std::string fixName(std::string name)
Definition: interaction_handler.cpp:115
Transform.h


robot_interaction
Author(s): Ioan Sucan
autogenerated on Sat Mar 15 2025 02:26:54