robot_interaction.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 <boost/algorithm/string.hpp>
50 
51 #include <algorithm>
52 #include <limits>
53 
54 #include <Eigen/Core>
55 #include <Eigen/Geometry>
56 
57 namespace robot_interaction
58 {
59 static const float END_EFFECTOR_UNREACHABLE_COLOR[4] = { 1.0, 0.3, 0.3, 1.0 };
60 static const float END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 };
61 
62 const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interaction_interactive_marker_topic";
63 
64 RobotInteraction::RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns)
65  : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap)
66 {
67  topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC;
69 
70  // spin a thread that will process feedback events
72  processing_thread_.reset(new boost::thread(boost::bind(&RobotInteraction::processingThread, this)));
73 }
74 
76 {
77  run_processing_thread_ = false;
79  processing_thread_->join();
80 
81  clear();
82  delete int_marker_server_;
83 }
84 
85 void RobotInteraction::decideActiveComponents(const std::string& group)
86 {
88 }
89 
91 {
92  decideActiveEndEffectors(group, style);
93  decideActiveJoints(group);
94  if (active_eef_.empty() && active_vj_.empty() && active_generic_.empty())
95  ROS_INFO_NAMED("robot_interaction", "No active joints or end effectors found for group '%s'. "
96  "Make sure you have defined an end effector in your SRDF file and that "
97  "kinematics.yaml is loaded in this node's namespace.",
98  group.c_str());
99 }
100 
102  const ProcessFeedbackFn& process, const InteractiveMarkerUpdateFn& update,
103  const std::string& name)
104 {
105  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
107  g.construct_marker = construct;
108  g.update_pose = update;
109  g.process_feedback = process;
110  // compute the suffix that will be added to the generated markers
111  g.marker_name_suffix = "_" + name + "_" + boost::lexical_cast<std::string>(active_generic_.size());
112  active_generic_.push_back(g);
113 }
114 
115 static const double DEFAULT_SCALE = 0.25;
116 double RobotInteraction::computeLinkMarkerSize(const std::string& link)
117 {
118  const robot_model::LinkModel* lm = robot_model_->getLinkModel(link);
119  double size = 0;
120 
121  while (lm)
122  {
123  Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin();
124  // drop largest extension and take norm of two remaining
125  Eigen::MatrixXd::Index maxIndex;
126  ext.maxCoeff(&maxIndex);
127  ext[maxIndex] = 0;
128  size = 1.01 * ext.norm();
129  if (size > 0)
130  break; // break, if a non-empty shape was found
131 
132  // process kinematic chain upwards (but only following fixed joints)
133  // to find a link with some non-empty shape (to ignore virtual links)
134  if (lm->getParentJointModel()->getType() == robot_model::JointModel::FIXED)
135  lm = lm->getParentLinkModel();
136  else
137  lm = 0;
138  }
139  if (!lm)
140  return DEFAULT_SCALE; // no link with non-zero shape extends found
141 
142  // the marker sphere will be half the size, so double the size here
143  return 2. * size;
144 }
145 
146 double RobotInteraction::computeGroupMarkerSize(const std::string& group)
147 {
148  if (group.empty())
149  return DEFAULT_SCALE;
150  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
151  if (!jmg)
152  return 0.0;
153 
154  const std::vector<std::string>& links = jmg->getLinkModelNames();
155  if (links.empty())
156  return DEFAULT_SCALE;
157 
158  // compute the aabb of the links that make up the group
159  double size = 0;
160  for (std::size_t i = 0; i < links.size(); ++i)
161  {
162  const robot_model::LinkModel* lm = robot_model_->getLinkModel(links[i]);
163  if (!lm)
164  continue;
165  Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin();
166 
167  // drop largest extension and take norm of two remaining
168  Eigen::MatrixXd::Index maxIndex;
169  ext.maxCoeff(&maxIndex);
170  ext[maxIndex] = 0;
171  size = std::max(size, 1.01 * ext.norm());
172  }
173 
174  // if size is zero, all links have empty shapes and are placed at same position
175  // in this case, fall back to link marker size
176  if (size == 0)
177  return computeLinkMarkerSize(links[0]);
178 
179  // the marker sphere will be half the size, so double the size here
180  return 2. * size;
181 }
182 
183 void RobotInteraction::decideActiveJoints(const std::string& group)
184 {
185  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
186  active_vj_.clear();
187 
188  ROS_DEBUG_NAMED("robot_interaction", "Deciding active joints for group '%s'", group.c_str());
189 
190  if (group.empty())
191  return;
192 
193  const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
194  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
195 
196  if (!jmg || !srdf)
197  return;
198 
199  std::set<std::string> used;
200  if (jmg->hasJointModel(robot_model_->getRootJointName()))
201  {
202  robot_state::RobotState default_state(robot_model_);
203  default_state.setToDefaultValues();
204  std::vector<double> aabb;
205  default_state.computeAABB(aabb);
206 
207  const std::vector<srdf::Model::VirtualJoint>& vj = srdf->getVirtualJoints();
208  for (std::size_t i = 0; i < vj.size(); ++i)
209  if (vj[i].name_ == robot_model_->getRootJointName())
210  {
211  if (vj[i].type_ == "planar" || vj[i].type_ == "floating")
212  {
214  v.connecting_link = vj[i].child_link_;
215  v.parent_frame = vj[i].parent_frame_;
216  if (!v.parent_frame.empty() && v.parent_frame[0] == '/')
217  v.parent_frame = v.parent_frame.substr(1);
218  v.joint_name = vj[i].name_;
219  if (vj[i].type_ == "planar")
220  v.dof = 3;
221  else
222  v.dof = 6;
223  // take the max of the X, Y, Z extent
224  v.size = std::max(std::max(aabb[1] - aabb[0], aabb[3] - aabb[2]), aabb[5] - aabb[4]);
225  active_vj_.push_back(v);
226  used.insert(v.joint_name);
227  }
228  }
229  }
230 
231  const std::vector<const robot_model::JointModel*>& joints = jmg->getJointModels();
232  for (std::size_t i = 0; i < joints.size(); ++i)
233  {
234  if ((joints[i]->getType() == robot_model::JointModel::PLANAR ||
235  joints[i]->getType() == robot_model::JointModel::FLOATING) &&
236  used.find(joints[i]->getName()) == used.end())
237  {
239  v.connecting_link = joints[i]->getChildLinkModel()->getName();
240  if (joints[i]->getParentLinkModel())
241  v.parent_frame = joints[i]->getParentLinkModel()->getName();
242  v.joint_name = joints[i]->getName();
243  if (joints[i]->getType() == robot_model::JointModel::PLANAR)
244  v.dof = 3;
245  else
246  v.dof = 6;
247  // take the max of the X, Y, Z extent
248  v.size = computeGroupMarkerSize(group);
249  active_vj_.push_back(v);
250  }
251  }
252 }
253 
254 void RobotInteraction::decideActiveEndEffectors(const std::string& group)
255 {
257 }
258 
260 {
261  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
262  active_eef_.clear();
263 
264  ROS_DEBUG_NAMED("robot_interaction", "Deciding active end-effectors for group '%s'", group.c_str());
265 
266  if (group.empty())
267  return;
268 
269  const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
270  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
271 
272  if (!jmg || !srdf)
273  {
274  ROS_WARN_NAMED("robot_interaction", "Unable to decide active end effector: no joint model group or no srdf model");
275  return;
276  }
277 
278  const std::vector<srdf::Model::EndEffector>& eef = srdf->getEndEffectors();
279  const std::pair<robot_model::JointModelGroup::KinematicsSolver, robot_model::JointModelGroup::KinematicsSolverMap>&
280  smap = jmg->getGroupKinematics();
281 
282  // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group
283  if (smap.first)
284  {
285  if (eef.empty() && !jmg->getLinkModelNames().empty())
286  {
287  // No end effectors. Use last link in group as the "end effector".
289  ee.parent_group = group;
290  ee.parent_link = jmg->getLinkModelNames().back();
291  ee.eef_group = group;
292  ee.interaction = style;
293  active_eef_.push_back(ee);
294  }
295  else
296  {
297  for (std::size_t i = 0; i < eef.size(); ++i)
298  if ((jmg->hasLinkModel(eef[i].parent_link_) || jmg->getName() == eef[i].parent_group_) &&
299  jmg->canSetStateFromIK(eef[i].parent_link_))
300  {
301  // We found an end-effector whose parent is the group.
303  ee.parent_group = group;
304  ee.parent_link = eef[i].parent_link_;
305  ee.eef_group = eef[i].component_group_;
306  ee.interaction = style;
307  active_eef_.push_back(ee);
308  }
309  }
310  }
311  else if (!smap.second.empty())
312  {
313  for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = smap.second.begin();
314  it != smap.second.end(); ++it)
315  {
316  for (std::size_t i = 0; i < eef.size(); ++i)
317  {
318  if ((it->first->hasLinkModel(eef[i].parent_link_) || jmg->getName() == eef[i].parent_group_) &&
319  it->first->canSetStateFromIK(eef[i].parent_link_))
320  {
321  // We found an end-effector whose parent is a subgroup of the group. (May be more than one)
323  ee.parent_group = it->first->getName();
324  ee.parent_link = eef[i].parent_link_;
325  ee.eef_group = eef[i].component_group_;
326  ee.interaction = style;
327  active_eef_.push_back(ee);
328  break;
329  }
330  }
331  }
332  }
333 
334  for (std::size_t i = 0; i < active_eef_.size(); ++i)
335  {
336  // if we have a separate group for the eef, we compute the scale based on it;
337  // otherwise, we use the size of the parent_link
338  active_eef_[i].size = active_eef_[i].eef_group == active_eef_[i].parent_group ?
339  computeLinkMarkerSize(active_eef_[i].parent_link) :
340  computeGroupMarkerSize(active_eef_[i].eef_group);
341  ROS_DEBUG_NAMED("robot_interaction", "Found active end-effector '%s', of scale %lf",
342  active_eef_[i].eef_group.c_str(), active_eef_[i].size);
343  }
344  // if there is only a single end effector marker, we can safely use a larger marker
345  if (active_eef_.size() == 1)
346  active_eef_[0].size *= 1.5;
347 }
348 
350 {
351  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
352  active_eef_.clear();
353  active_vj_.clear();
354  active_generic_.clear();
357 }
358 
360 {
361  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
363 }
364 
366 {
367  handlers_.clear();
368  shown_markers_.clear();
370  int_marker_move_topics_.clear();
371  int_marker_names_.clear();
373 }
374 
375 void RobotInteraction::addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
376  const EndEffectorInteraction& eef,
377  visualization_msgs::InteractiveMarker& im, bool position, bool orientation)
378 {
379  geometry_msgs::Pose pose;
380  pose.orientation.w = 1;
381  addEndEffectorMarkers(handler, eef, pose, im, position, orientation);
382 }
383 
384 void RobotInteraction::addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
385  const EndEffectorInteraction& eef, const geometry_msgs::Pose& im_to_eef,
386  visualization_msgs::InteractiveMarker& im, bool position, bool orientation)
387 {
388  if (eef.parent_group == eef.eef_group || !robot_model_->hasLinkModel(eef.parent_link))
389  return;
390 
391  visualization_msgs::InteractiveMarkerControl m_control;
392  m_control.always_visible = false;
393  if (position && orientation)
394  m_control.interaction_mode = m_control.MOVE_ROTATE_3D;
395  else if (orientation)
396  m_control.interaction_mode = m_control.ROTATE_3D;
397  else
398  m_control.interaction_mode = m_control.MOVE_3D;
399 
400  std_msgs::ColorRGBA marker_color;
401  const float* color = handler->inError(eef) ? END_EFFECTOR_UNREACHABLE_COLOR : END_EFFECTOR_REACHABLE_COLOR;
402  marker_color.r = color[0];
403  marker_color.g = color[1];
404  marker_color.b = color[2];
405  marker_color.a = color[3];
406 
407  robot_state::RobotStateConstPtr rstate = handler->getState();
408  const std::vector<std::string>& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames();
409  visualization_msgs::MarkerArray marker_array;
410  rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, ros::Duration());
411  tf::Pose tf_root_to_link;
412  tf::poseEigenToTF(rstate->getGlobalLinkTransform(eef.parent_link), tf_root_to_link);
413  // Release the ptr count on the kinematic state
414  rstate.reset();
415 
416  for (std::size_t i = 0; i < marker_array.markers.size(); ++i)
417  {
418  marker_array.markers[i].header = im.header;
419  marker_array.markers[i].mesh_use_embedded_materials = true;
420  // - - - - - - Do some math for the offset - - - - - -
421  tf::Pose tf_root_to_im, tf_root_to_mesh, tf_im_to_eef;
422  tf::poseMsgToTF(im.pose, tf_root_to_im);
423  tf::poseMsgToTF(marker_array.markers[i].pose, tf_root_to_mesh);
424  tf::poseMsgToTF(im_to_eef, tf_im_to_eef);
425  tf::Pose tf_eef_to_mesh = tf_root_to_link.inverse() * tf_root_to_mesh;
426  tf::Pose tf_im_to_mesh = tf_im_to_eef * tf_eef_to_mesh;
427  tf::Pose tf_root_to_mesh_new = tf_root_to_im * tf_im_to_mesh;
428  tf::poseTFToMsg(tf_root_to_mesh_new, marker_array.markers[i].pose);
429  // - - - - - - - - - - - - - - - - - - - - - - - - - -
430  m_control.markers.push_back(marker_array.markers[i]);
431  }
432 
433  im.controls.push_back(m_control);
434 }
435 
436 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
437  const EndEffectorInteraction& eef)
438 {
439  return "EE:" + handler->getName() + "_" + eef.parent_link;
440 }
441 
442 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
443  const JointInteraction& vj)
444 {
445  return "JJ:" + handler->getName() + "_" + vj.connecting_link;
446 }
447 
448 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
449  const GenericInteraction& g)
450 {
451  return "GG:" + handler->getName() + "_" + g.marker_name_suffix;
452 }
453 
454 void RobotInteraction::addInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
455  const double marker_scale)
456 {
457  handler->setRobotInteraction(this);
458  // If scale is left at default size of 0, scale will be based on end effector link size. a good value is between 0-1
459  std::vector<visualization_msgs::InteractiveMarker> ims;
460  ros::NodeHandle nh;
461  {
462  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
463  robot_state::RobotStateConstPtr s = handler->getState();
464 
465  for (std::size_t i = 0; i < active_generic_.size(); ++i)
466  {
467  visualization_msgs::InteractiveMarker im;
468  if (active_generic_[i].construct_marker(*s, im))
469  {
470  im.name = getMarkerName(handler, active_generic_[i]);
471  shown_markers_[im.name] = i;
472  ims.push_back(im);
473  ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(),
474  im.scale);
475  }
476  }
477  ros::NodeHandle nh;
478 
479  for (std::size_t i = 0; i < active_eef_.size(); ++i)
480  {
481  geometry_msgs::PoseStamped pose;
482  geometry_msgs::Pose control_to_eef_tf;
483  pose.header.frame_id = robot_model_->getModelFrame();
484  pose.header.stamp = ros::Time::now();
485  computeMarkerPose(handler, active_eef_[i], *s, pose.pose, control_to_eef_tf);
486 
487  std::string marker_name = getMarkerName(handler, active_eef_[i]);
488  shown_markers_[marker_name] = i;
489 
490  // Determine interactive maker size
491  double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_eef_[i].size : marker_scale;
492 
493  visualization_msgs::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
494  if (handler && handler->getControlsVisible())
495  {
496  if (active_eef_[i].interaction & EEF_POSITION_ARROWS)
497  addPositionControl(im, active_eef_[i].interaction & EEF_FIXED);
498  if (active_eef_[i].interaction & EEF_ORIENTATION_CIRCLES)
499  addOrientationControl(im, active_eef_[i].interaction & EEF_FIXED);
500  if (active_eef_[i].interaction & (EEF_POSITION_SPHERE | EEF_ORIENTATION_SPHERE))
501  {
502  std_msgs::ColorRGBA color;
503  color.r = 0;
504  color.g = 1;
505  color.b = 1;
506  color.a = 0.5;
507  addViewPlaneControl(im, mscale * 0.25, color, active_eef_[i].interaction & EEF_POSITION_SPHERE,
508  active_eef_[i].interaction & EEF_ORIENTATION_SPHERE);
509  }
510  }
511  if (handler && handler->getMeshesVisible() &&
512  (active_eef_[i].interaction & (EEF_POSITION_EEF | EEF_ORIENTATION_EEF)))
513  addEndEffectorMarkers(handler, active_eef_[i], control_to_eef_tf, im,
514  active_eef_[i].interaction & EEF_POSITION_EEF,
515  active_eef_[i].interaction & EEF_ORIENTATION_EEF);
516  ims.push_back(im);
517  registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link);
518  ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(),
519  mscale);
520  }
521  for (std::size_t i = 0; i < active_vj_.size(); ++i)
522  {
523  geometry_msgs::PoseStamped pose;
524  pose.header.frame_id = robot_model_->getModelFrame();
525  pose.header.stamp = ros::Time::now();
526  tf::poseEigenToMsg(s->getGlobalLinkTransform(active_vj_[i].connecting_link), pose.pose);
527  std::string marker_name = getMarkerName(handler, active_vj_[i]);
528  shown_markers_[marker_name] = i;
529 
530  // Determine interactive maker size
531  double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_vj_[i].size : marker_scale;
532 
533  visualization_msgs::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
534  if (handler && handler->getControlsVisible())
535  {
536  if (active_vj_[i].dof == 3) // planar joint
537  addPlanarXYControl(im, false);
538  else
539  add6DOFControl(im, false);
540  }
541  ims.push_back(im);
542  registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link);
543  ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(),
544  mscale);
545  }
546  handlers_[handler->getName()] = handler;
547  }
548 
549  // we do this while marker_access_lock_ is unlocked because the interactive marker server locks
550  // for most function calls, and maintains that lock while the feedback callback is running
551  // that can cause a deadlock if we were to run the loop below while marker_access_lock_ is locked
552  for (std::size_t i = 0; i < ims.size(); ++i)
553  {
554  int_marker_server_->insert(ims[i]);
555  int_marker_server_->setCallback(ims[i].name,
557 
558  // Add menu handler to all markers that this interaction handler creates.
559  if (std::shared_ptr<interactive_markers::MenuHandler> mh = handler->getMenuHandler())
560  mh->apply(*int_marker_server_, ims[i].name);
561  }
562 }
563 
564 void RobotInteraction::registerMoveInteractiveMarkerTopic(const std::string marker_name, const std::string& name)
565 {
566  ros::NodeHandle nh;
567  std::stringstream ss;
568  ss << "/rviz/moveit/move_marker/";
569  ss << name;
570  int_marker_move_topics_.push_back(ss.str());
571  int_marker_names_.push_back(marker_name);
572 }
573 
575 {
576  if (enable)
577  {
578  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
579  if (int_marker_move_subscribers_.size() == 0)
580  {
581  ros::NodeHandle nh;
582  for (size_t i = 0; i < int_marker_move_topics_.size(); i++)
583  {
584  std::string topic_name = int_marker_move_topics_[i];
585  std::string marker_name = int_marker_names_[i];
586  int_marker_move_subscribers_.push_back(nh.subscribe<geometry_msgs::PoseStamped>(
587  topic_name, 1, boost::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, _1)));
588  }
589  }
590  }
591  else
592  {
593  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
595  }
596 }
597 
598 void RobotInteraction::computeMarkerPose(const ::robot_interaction::InteractionHandlerPtr& handler,
599  const EndEffectorInteraction& eef, const robot_state::RobotState& robot_state,
600  geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const
601 {
602  // Need to allow for control pose offsets
603  tf::Transform tf_root_to_link, tf_root_to_control;
604  tf::poseEigenToTF(robot_state.getGlobalLinkTransform(eef.parent_link), tf_root_to_link);
605 
606  geometry_msgs::Pose msg_link_to_control;
607  if (handler->getPoseOffset(eef, msg_link_to_control))
608  {
609  tf::Transform tf_link_to_control;
610  tf::poseMsgToTF(msg_link_to_control, tf_link_to_control);
611 
612  tf_root_to_control = tf_root_to_link * tf_link_to_control;
613  tf::poseTFToMsg(tf_link_to_control.inverse(), control_to_eef_tf);
614  }
615  else
616  {
617  tf_root_to_control = tf_root_to_link;
618  control_to_eef_tf.orientation.x = 0.0;
619  control_to_eef_tf.orientation.y = 0.0;
620  control_to_eef_tf.orientation.z = 0.0;
621  control_to_eef_tf.orientation.w = 1.0;
622  }
623 
624  tf::poseTFToMsg(tf_root_to_control, pose);
625 }
626 
627 void RobotInteraction::updateInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler)
628 {
629  handler->setRobotInteraction(this);
630  std::string root_link;
631  std::map<std::string, geometry_msgs::Pose> pose_updates;
632  {
633  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
634 
635  robot_state::RobotStateConstPtr s = handler->getState();
636  root_link = s->getRobotModel()->getModelFrame();
637 
638  for (std::size_t i = 0; i < active_eef_.size(); ++i)
639  {
640  std::string marker_name = getMarkerName(handler, active_eef_[i]);
641  geometry_msgs::Pose control_to_eef_tf;
642  computeMarkerPose(handler, active_eef_[i], *s, pose_updates[marker_name], control_to_eef_tf);
643  }
644 
645  for (std::size_t i = 0; i < active_vj_.size(); ++i)
646  {
647  std::string marker_name = getMarkerName(handler, active_vj_[i]);
648  tf::poseEigenToMsg(s->getGlobalLinkTransform(active_vj_[i].connecting_link), pose_updates[marker_name]);
649  }
650 
651  for (std::size_t i = 0; i < active_generic_.size(); ++i)
652  {
653  std::string marker_name = getMarkerName(handler, active_generic_[i]);
654  geometry_msgs::Pose pose;
655  if (active_generic_[i].update_pose && active_generic_[i].update_pose(*s, pose))
656  pose_updates[marker_name] = pose;
657  }
658  }
659 
660  std_msgs::Header header;
661  header.frame_id = root_link; // marker poses are give w.r.t. root frame
662  for (std::map<std::string, geometry_msgs::Pose>::const_iterator it = pose_updates.begin(); it != pose_updates.end();
663  ++it)
664  int_marker_server_->setPose(it->first, it->second, header);
666 }
667 
669 {
670  // the server locks internally, so we need not worry about locking
672 }
673 
674 bool RobotInteraction::showingMarkers(const ::robot_interaction::InteractionHandlerPtr& handler)
675 {
676  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
677 
678  for (std::size_t i = 0; i < active_eef_.size(); ++i)
679  if (shown_markers_.find(getMarkerName(handler, active_eef_[i])) == shown_markers_.end())
680  return false;
681  for (std::size_t i = 0; i < active_vj_.size(); ++i)
682  if (shown_markers_.find(getMarkerName(handler, active_vj_[i])) == shown_markers_.end())
683  return false;
684  for (std::size_t i = 0; i < active_generic_.size(); ++i)
685  if (shown_markers_.find(getMarkerName(handler, active_generic_[i])) == shown_markers_.end())
686  return false;
687  return true;
688 }
689 
690 // TODO: can we get rid of this? Only used in moveit_ros/benchmarks_gui/src/tab_states_and_goals.cpp right now.
691 bool RobotInteraction::updateState(robot_state::RobotState& state, const EndEffectorInteraction& eef,
692  const geometry_msgs::Pose& pose, unsigned int attempts, double ik_timeout,
693  const robot_state::GroupStateValidityCallbackFn& validity_callback,
694  const kinematics::KinematicsQueryOptions& kinematics_query_options)
695 {
696  if (state.setFromIK(state.getJointModelGroup(eef.parent_group), pose, eef.parent_link,
697  kinematics_query_options.lock_redundant_joints ? 1 : attempts, ik_timeout, validity_callback,
698  kinematics_query_options))
699  {
700  state.update();
701  return true;
702  }
703  return false;
704 }
705 
706 void RobotInteraction::moveInteractiveMarker(const std::string name, const geometry_msgs::PoseStampedConstPtr& msg)
707 {
708  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(name);
709  if (it != shown_markers_.end())
710  {
711  visualization_msgs::InteractiveMarkerFeedback::Ptr feedback(new visualization_msgs::InteractiveMarkerFeedback);
712  feedback->header = msg->header;
713  feedback->marker_name = name;
714  feedback->pose = msg->pose;
715  feedback->event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
717  {
718  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
719  int_marker_server_->setPose(name, msg->pose, msg->header); // move the interactive marker
721  }
722  }
723 }
724 
726  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
727 {
728  // perform some validity checks
729  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
730  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
731  if (it == shown_markers_.end())
732  {
733  ROS_ERROR("Unknown marker name: '%s' (not published by RobotInteraction class)", feedback->marker_name.c_str());
734  return;
735  }
736 
737  std::size_t u = feedback->marker_name.find_first_of("_");
738  if (u == std::string::npos || u < 4)
739  {
740  ROS_ERROR("Invalid marker name: '%s'", feedback->marker_name.c_str());
741  return;
742  }
743 
744  feedback_map_[feedback->marker_name] = feedback;
746 }
747 
749 {
750  boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
751 
752  while (run_processing_thread_ && ros::ok())
753  {
754  while (feedback_map_.empty() && run_processing_thread_ && ros::ok())
756 
757  while (!feedback_map_.empty() && ros::ok())
758  {
759  visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback = feedback_map_.begin()->second;
760  feedback_map_.erase(feedback_map_.begin());
761  ROS_DEBUG_NAMED("robot_interaction", "Processing feedback from map for marker [%s]",
762  feedback->marker_name.c_str());
763 
764  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
765  if (it == shown_markers_.end())
766  {
767  ROS_ERROR("Unknown marker name: '%s' (not published by RobotInteraction class) "
768  "(should never have ended up in the feedback_map!)",
769  feedback->marker_name.c_str());
770  continue;
771  }
772  std::size_t u = feedback->marker_name.find_first_of("_");
773  if (u == std::string::npos || u < 4)
774  {
775  ROS_ERROR("Invalid marker name: '%s' (should never have ended up in the feedback_map!)",
776  feedback->marker_name.c_str());
777  continue;
778  }
779  std::string marker_class = feedback->marker_name.substr(0, 2);
780  std::string handler_name = feedback->marker_name.substr(3, u - 3); // skip the ":"
781  std::map<std::string, ::robot_interaction::InteractionHandlerPtr>::const_iterator jt =
782  handlers_.find(handler_name);
783  if (jt == handlers_.end())
784  {
785  ROS_ERROR("Interactive Marker Handler '%s' is not known.", handler_name.c_str());
786  continue;
787  }
788 
789  // we put this in a try-catch because user specified callbacks may be triggered
790  try
791  {
792  if (marker_class == "EE")
793  {
794  // make a copy of the data, so we do not lose it while we are unlocked
795  EndEffectorInteraction eef = active_eef_[it->second];
796  ::robot_interaction::InteractionHandlerPtr ih = jt->second;
797  marker_access_lock_.unlock();
798  try
799  {
800  ih->handleEndEffector(eef, feedback);
801  }
802  catch (std::exception& ex)
803  {
804  ROS_ERROR("Exception caught while handling end-effector update: %s", ex.what());
805  }
806  marker_access_lock_.lock();
807  }
808  else if (marker_class == "JJ")
809  {
810  // make a copy of the data, so we do not lose it while we are unlocked
811  JointInteraction vj = active_vj_[it->second];
812  ::robot_interaction::InteractionHandlerPtr ih = jt->second;
813  marker_access_lock_.unlock();
814  try
815  {
816  ih->handleJoint(vj, feedback);
817  }
818  catch (std::exception& ex)
819  {
820  ROS_ERROR("Exception caught while handling joint update: %s", ex.what());
821  }
822  marker_access_lock_.lock();
823  }
824  else if (marker_class == "GG")
825  {
826  ::robot_interaction::InteractionHandlerPtr ih = jt->second;
827  GenericInteraction g = active_generic_[it->second];
828  marker_access_lock_.unlock();
829  try
830  {
831  ih->handleGeneric(g, feedback);
832  }
833  catch (std::exception& ex)
834  {
835  ROS_ERROR("Exception caught while handling joint update: %s", ex.what());
836  }
837  marker_access_lock_.lock();
838  }
839  else
840  ROS_ERROR("Unknown marker class ('%s') for marker '%s'", marker_class.c_str(), feedback->marker_name.c_str());
841  }
842  catch (std::exception& ex)
843  {
844  ROS_ERROR("Exception caught while processing event: %s", ex.what());
845  }
846  }
847  }
848 }
849 
850 // DEPRECATED FUNCTIONALITY for backwards compatibility
852 {
854 }
855 
856 // DEPRECATED FUNCTIONALITY for backwards compatibility
858 {
860 }
861 }
boost::condition_variable new_feedback_condition_
std::vector< JointInteraction > active_vj_
void moveInteractiveMarker(const std::string name, const geometry_msgs::PoseStampedConstPtr &msg)
void notify_all() BOOST_NOEXCEPT
visualization_msgs::InteractiveMarker makeEmptyInteractiveMarker(const std::string &name, const geometry_msgs::PoseStamped &stamped, double scale)
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
#define ROS_INFO_NAMED(name,...)
std::string parent_link
The name of the link in the parent group that connects to the end-effector.
Definition: interaction.h:147
InteractionStyle::InteractionStyle interaction
Which degrees of freedom to enable for the end-effector.
Definition: interaction.h:153
std::map< std::string,::robot_interaction::InteractionHandlerPtr > handlers_
#define ROS_WARN_NAMED(name,...)
void registerMoveInteractiveMarkerTopic(const std::string marker_name, const std::string &name)
void wait(unique_lock< mutex > &m)
double computeGroupMarkerSize(const std::string &group)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void addPlanarXYControl(visualization_msgs::InteractiveMarker &int_marker, bool orientation_fixed=false)
void decideActiveJoints(const std::string &group)
XmlRpcServer s
static bool updateState(robot_state::RobotState &state, const EndEffectorInteraction &eef, const geometry_msgs::Pose &pose, unsigned int attempts, double ik_timeout, const robot_state::GroupStateValidityCallbackFn &validity_callback=robot_state::GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &kinematics_query_options=kinematics::KinematicsQueryOptions())
void addPositionControl(visualization_msgs::InteractiveMarker &int_marker, bool orientation_fixed=false)
std::string getName(void *handle)
std::vector< ros::Subscriber > int_marker_move_subscribers_
static const float END_EFFECTOR_UNREACHABLE_COLOR[4]
void addViewPlaneControl(visualization_msgs::InteractiveMarker &int_marker, double radius, const std_msgs::ColorRGBA &color, bool position=true, bool orientation=true)
double size
The size of the connecting link (diameter of enclosing sphere)
Definition: interaction.h:176
static const double DEFAULT_SCALE
double computeLinkMarkerSize(const std::string &link)
#define ROS_DEBUG_NAMED(name,...)
std::vector< std::string > int_marker_names_
robot_model::RobotModelConstPtr robot_model_
interactive_markers::InteractiveMarkerServer * int_marker_server_
void addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr &handler, const EndEffectorInteraction &eef, visualization_msgs::InteractiveMarker &im, bool position=true, bool orientation=true)
std::string eef_group
The name of the group that defines the group joints.
Definition: interaction.h:150
void addOrientationControl(visualization_msgs::InteractiveMarker &int_marker, bool orientation_fixed=false)
void decideActiveEndEffectors(const std::string &group)
std::vector< std::string > int_marker_move_topics_
static const float END_EFFECTOR_REACHABLE_COLOR[4]
ROSCPP_DECL bool ok()
std::map< std::string, visualization_msgs::InteractiveMarkerFeedbackConstPtr > feedback_map_
std::string parent_group
The name of the group that sustains the end-effector (usually an arm)
Definition: interaction.h:144
boost::function< bool(const robot_state::RobotState &, geometry_msgs::Pose &)> InteractiveMarkerUpdateFn
Definition: interaction.h:116
Transform inverse() const
boost::function< bool(robot_state::RobotState &state, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)> ProcessFeedbackFn
Definition: interaction.h:105
RobotInteraction(const robot_model::RobotModelConstPtr &kmodel, const std::string &ns="")
void processInteractiveMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
static std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr &handler, const EndEffectorInteraction &eef)
boost::function< bool(const robot_state::RobotState &state, visualization_msgs::InteractiveMarker &marker)> InteractiveMarkerConstructorFn
Definition: interaction.h:90
void add6DOFControl(visualization_msgs::InteractiveMarker &int_marker, bool orientation_fixed=false)
void computeMarkerPose(const ::robot_interaction::InteractionHandlerPtr &handler, const EndEffectorInteraction &eef, const robot_state::RobotState &robot_state, geometry_msgs::Pose &pose, geometry_msgs::Pose &control_to_eef_tf) const
std::string connecting_link
The link in the robot model this joint is a parent of.
Definition: interaction.h:164
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 insert(const visualization_msgs::InteractiveMarker &int_marker)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB)
void addInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr &handler, const double marker_scale=0.0)
static Time now()
InteractiveMarkerConstructorFn construct_marker
Definition: interaction.h:124
std::vector< EndEffectorInteraction > active_eef_
bool showingMarkers(const ::robot_interaction::InteractionHandlerPtr &handler)
bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
std::unique_ptr< boost::thread > processing_thread_
std::string joint_name
The name of the joint.
Definition: interaction.h:170
std::map< std::string, std::size_t > shown_markers_
std::vector< GenericInteraction > active_generic_
void updateInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr &handler)
#define ROS_ERROR(...)
InteractiveMarkerUpdateFn update_pose
Definition: interaction.h:132
void addActiveComponent(const InteractiveMarkerConstructorFn &construct, const ProcessFeedbackFn &process, const InteractiveMarkerUpdateFn &update=InteractiveMarkerUpdateFn(), const std::string &name="")
static const std::string INTERACTIVE_MARKER_TOPIC
The topic name on which the internal Interactive Marker Server operates.
void decideActiveComponents(const std::string &group)


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