moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
43 #include <interactive_markers/interactive_marker_server.hpp>
44 #include <interactive_markers/menu_handler.hpp>
45 #include <tf2_eigen/tf2_eigen.hpp>
46 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
47 #include <tf2/LinearMath/Transform.h>
48 
49 #include <algorithm>
50 #include <limits>
51 #include <string>
52 
53 #include <Eigen/Core>
54 #include <Eigen/Geometry>
55 
56 namespace robot_interaction
57 {
58 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.robot_interaction");
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 moveit::core::RobotModelConstPtr& robot_model,
65  const rclcpp::Node::SharedPtr& node, const std::string& ns)
66  : robot_model_(robot_model), kinematic_options_map_(std::make_shared<KinematicOptionsMap>())
67 {
68  topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC;
69  node_ = node;
70  int_marker_server_ = new interactive_markers::InteractiveMarkerServer(topic_, node_);
71 
72  // spin a thread that will process feedback events
73  run_processing_thread_ = true;
74  processing_thread_ = std::make_unique<std::thread>([this] { processingThread(); });
75 }
76 
78 {
79  run_processing_thread_ = false;
80  new_feedback_condition_.notify_all();
81  processing_thread_->join();
82 
83  clear();
84  delete int_marker_server_;
85 }
86 
88 {
90 }
91 
93 {
94  decideActiveEndEffectors(group, style);
95  decideActiveJoints(group);
96  if (!group.empty() && active_eef_.empty() && active_vj_.empty() && active_generic_.empty())
97  RCLCPP_INFO(LOGGER,
98  "No active joints or end effectors found for group '%s'. "
99  "Make sure that kinematics.yaml is loaded in this node's namespace.",
100  group.c_str());
101 }
102 
104  const ProcessFeedbackFn& process, const InteractiveMarkerUpdateFn& update,
105  const std::string& name)
106 {
107  std::unique_lock<std::mutex> ulock(marker_access_lock_);
109  g.construct_marker = construct;
110  g.update_pose = update;
111  g.process_feedback = process;
112  // compute the suffix that will be added to the generated markers
113  g.marker_name_suffix = "_" + name + "_" + std::to_string(active_generic_.size());
114  active_generic_.push_back(g);
115 }
116 
117 static const double DEFAULT_SCALE = 0.25;
118 double RobotInteraction::computeLinkMarkerSize(const std::string& link)
119 {
120  const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link);
121  double size = 0;
122 
123  while (lm)
124  {
126  // drop largest extension and take norm of two remaining
127  Eigen::MatrixXd::Index max_index;
128  ext.maxCoeff(&max_index);
129  ext[max_index] = 0;
130  size = 1.01 * ext.norm();
131  if (size > 0)
132  break; // break, if a non-empty shape was found
133 
134  // process kinematic chain upwards (but only following fixed joints)
135  // to find a link with some non-empty shape (to ignore virtual links)
137  lm = lm->getParentLinkModel();
138  else
139  lm = nullptr;
140  }
141  if (!lm)
142  return DEFAULT_SCALE; // no link with non-zero shape extends found
143 
144  // the marker sphere will be half the size, so double the size here
145  return 2. * size;
146 }
147 
148 double RobotInteraction::computeGroupMarkerSize(const std::string& group)
149 {
150  if (group.empty())
151  return DEFAULT_SCALE;
152  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
153  if (!jmg)
154  return 0.0;
155 
156  const std::vector<std::string>& links = jmg->getLinkModelNames();
157  if (links.empty())
158  return DEFAULT_SCALE;
159 
160  // compute the aabb of the links that make up the group
161  double size = 0;
162  for (const std::string& link : links)
163  {
164  const moveit::core::LinkModel* lm = robot_model_->getLinkModel(link);
165  if (!lm)
166  continue;
168 
169  // drop largest extension and take norm of two remaining
170  Eigen::MatrixXd::Index max_index;
171  ext.maxCoeff(&max_index);
172  ext[max_index] = 0;
173  size = std::max(size, 1.01 * ext.norm());
174  }
175 
176  // if size is zero, all links have empty shapes and are placed at same position
177  // in this case, fall back to link marker size
178  if (size == 0)
179  return computeLinkMarkerSize(links[0]);
180 
181  // the marker sphere will be half the size, so double the size here
182  return 2. * size;
183 }
184 
185 void RobotInteraction::decideActiveJoints(const std::string& group)
186 {
187  std::unique_lock<std::mutex> ulock(marker_access_lock_);
188  active_vj_.clear();
189 
190  if (group.empty())
191  return;
192 
193  RCLCPP_DEBUG(LOGGER, "Deciding active joints for group '%s'", group.c_str());
194 
195  const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
196  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
197 
198  if (!jmg || !srdf)
199  return;
200 
201  std::set<std::string> used;
202  if (jmg->hasJointModel(robot_model_->getRootJointName()))
203  {
204  moveit::core::RobotState default_state(robot_model_);
205  default_state.setToDefaultValues();
206  std::vector<double> aabb;
207  default_state.computeAABB(aabb);
208 
209  const std::vector<srdf::Model::VirtualJoint>& vj = srdf->getVirtualJoints();
210  for (const srdf::Model::VirtualJoint& joint : vj)
211  if (joint.name_ == robot_model_->getRootJointName())
212  {
213  if (joint.type_ == "planar" || joint.type_ == "floating")
214  {
215  JointInteraction v;
216  v.connecting_link = joint.child_link_;
217  v.parent_frame = joint.parent_frame_;
218  if (!v.parent_frame.empty() && v.parent_frame[0] == '/')
219  v.parent_frame = v.parent_frame.substr(1);
220  v.joint_name = joint.name_;
221  if (joint.type_ == "planar")
222  v.dof = 3;
223  else
224  v.dof = 6;
225  // take the max of the X, Y, Z extent
226  v.size = std::max(std::max(aabb[1] - aabb[0], aabb[3] - aabb[2]), aabb[5] - aabb[4]);
227  active_vj_.push_back(v);
228  used.insert(v.joint_name);
229  }
230  }
231  }
232 
233  const std::vector<const moveit::core::JointModel*>& joints = jmg->getJointModels();
234  for (const moveit::core::JointModel* joint : joints)
235  {
236  if ((joint->getType() == moveit::core::JointModel::PLANAR ||
237  joint->getType() == moveit::core::JointModel::FLOATING) &&
238  used.find(joint->getName()) == used.end())
239  {
240  JointInteraction v;
241  v.connecting_link = joint->getChildLinkModel()->getName();
242  if (joint->getParentLinkModel())
243  v.parent_frame = joint->getParentLinkModel()->getName();
244  v.joint_name = joint->getName();
245  if (joint->getType() == moveit::core::JointModel::PLANAR)
246  v.dof = 3;
247  else
248  v.dof = 6;
249  // take the max of the X, Y, Z extent
250  v.size = computeGroupMarkerSize(group);
251  active_vj_.push_back(v);
252  }
253  }
254 }
255 
256 void RobotInteraction::decideActiveEndEffectors(const std::string& group)
257 {
258  decideActiveEndEffectors(group, InteractionStyle::SIX_DOF);
259 }
260 
261 void RobotInteraction::decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style)
262 {
263  std::unique_lock<std::mutex> ulock(marker_access_lock_);
264  active_eef_.clear();
265 
266  if (group.empty())
267  return;
268 
269  RCLCPP_DEBUG(LOGGER, "Deciding active end-effectors for group '%s'", group.c_str());
270 
271  const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
272  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
273 
274  if (!jmg || !srdf)
275  {
276  RCLCPP_WARN(LOGGER, "Unable to decide active end effector: no joint model group or no srdf model");
277  return;
278  }
279 
280  const std::vector<srdf::Model::EndEffector>& eefs = srdf->getEndEffectors();
281  const std::pair<moveit::core::JointModelGroup::KinematicsSolver, moveit::core::JointModelGroup::KinematicsSolverMap>&
282  smap = jmg->getGroupKinematics();
283 
284  auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) {
285  bool found_eef{ false };
286  for (const srdf::Model::EndEffector& eef : eefs)
287  {
288  if (single_group->hasLinkModel(eef.parent_link_) &&
289  (eef.parent_group_.empty() || single_group->getName() == eef.parent_group_) &&
290  single_group->canSetStateFromIK(eef.parent_link_))
291  {
292  // We found an end-effector whose parent is the group.
293  EndEffectorInteraction ee;
294  ee.parent_group = single_group->getName();
295  ee.parent_link = eef.parent_link_;
296  ee.eef_group = eef.component_group_;
297  ee.interaction = style;
298  active_eef_.push_back(ee);
299  found_eef = true;
300  }
301  }
302 
303  // No end effectors found. Use last link in group as the "end effector".
304  if (!found_eef && !single_group->getLinkModelNames().empty())
305  {
306  std::string last_link{ single_group->getLinkModelNames().back() };
307 
308  if (single_group->canSetStateFromIK(last_link))
309  {
310  EndEffectorInteraction ee;
311  ee.parent_group = single_group->getName();
312  ee.parent_link = last_link;
313  ee.eef_group = single_group->getName();
314  ee.interaction = style;
315  active_eef_.push_back(ee);
316  }
317  }
318  };
319 
320  // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group
321  if (smap.first)
322  {
323  add_active_end_effectors_for_single_group(jmg);
324  }
325  // if the group contains subgroups with IK, add markers for them individually
326  else if (!smap.second.empty())
327  {
328  for (const std::pair<const moveit::core::JointModelGroup* const, moveit::core::JointModelGroup::KinematicsSolver>&
329  it : smap.second)
330  add_active_end_effectors_for_single_group(it.first);
331  }
332 
333  // lastly determine automatic marker sizes
334  for (EndEffectorInteraction& eef : active_eef_)
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  eef.size = (eef.eef_group == eef.parent_group) ? computeLinkMarkerSize(eef.parent_link) :
339  computeGroupMarkerSize(eef.eef_group);
340  RCLCPP_DEBUG(LOGGER, "Found active end-effector '%s', of scale %lf", eef.eef_group.c_str(), eef.size);
341  }
342  // if there is only a single end effector marker, we can safely use a larger marker
343  if (active_eef_.size() == 1)
344  active_eef_[0].size *= 1.5;
345 }
346 
348 {
349  std::unique_lock<std::mutex> ulock(marker_access_lock_);
350  active_eef_.clear();
351  active_vj_.clear();
352  active_generic_.clear();
353  clearInteractiveMarkersUnsafe();
355 }
356 
358 {
359  std::unique_lock<std::mutex> ulock(marker_access_lock_);
360  clearInteractiveMarkersUnsafe();
361 }
362 
363 void RobotInteraction::clearInteractiveMarkersUnsafe()
364 {
365  handlers_.clear();
366  shown_markers_.clear();
367  int_marker_move_subscribers_.clear();
368  int_marker_move_topics_.clear();
369  int_marker_names_.clear();
370  int_marker_server_->clear();
371 }
372 
373 void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
374  visualization_msgs::msg::InteractiveMarker& im, bool position,
375  bool orientation)
376 {
377  geometry_msgs::msg::Pose pose;
378  pose.orientation.w = 1;
379  addEndEffectorMarkers(handler, eef, pose, im, position, orientation);
380 }
381 
382 void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
383  const geometry_msgs::msg::Pose& im_to_eef,
384  visualization_msgs::msg::InteractiveMarker& im, bool position,
385  bool orientation)
386 {
387  if (eef.parent_group == eef.eef_group || !robot_model_->hasLinkModel(eef.parent_link))
388  return;
389 
390  visualization_msgs::msg::InteractiveMarkerControl m_control;
391  m_control.always_visible = false;
392  if (position && orientation)
393  m_control.interaction_mode = m_control.MOVE_ROTATE_3D;
394  else if (orientation)
395  m_control.interaction_mode = m_control.ROTATE_3D;
396  else
397  m_control.interaction_mode = m_control.MOVE_3D;
398 
399  std_msgs::msg::ColorRGBA marker_color;
400  const float* color = handler->inError(eef) ? END_EFFECTOR_UNREACHABLE_COLOR : END_EFFECTOR_REACHABLE_COLOR;
401  marker_color.r = color[0];
402  marker_color.g = color[1];
403  marker_color.b = color[2];
404  marker_color.a = color[3];
405 
406  moveit::core::RobotStateConstPtr rstate = handler->getState();
407  const std::vector<std::string>& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames();
408  visualization_msgs::msg::MarkerArray marker_array;
409  rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, rclcpp::Duration::from_seconds(0));
410  tf2::Transform tf_root_to_link;
411  tf2::fromMsg(tf2::toMsg(rstate->getGlobalLinkTransform(eef.parent_link)), tf_root_to_link);
412  // Release the ptr count on the kinematic state
413  rstate.reset();
414 
415  for (visualization_msgs::msg::Marker& marker : marker_array.markers)
416  {
417  marker.header = im.header;
418  marker.mesh_use_embedded_materials = !marker.mesh_resource.empty();
419  // - - - - - - Do some math for the offset - - - - - -
420  tf2::Transform tf_root_to_im, tf_root_to_mesh, tf_im_to_eef;
421  tf2::fromMsg(im.pose, tf_root_to_im);
422  tf2::fromMsg(marker.pose, tf_root_to_mesh);
423  tf2::fromMsg(im_to_eef, tf_im_to_eef);
424  tf2::Transform tf_eef_to_mesh = tf_root_to_link.inverse() * tf_root_to_mesh;
425  tf2::Transform tf_im_to_mesh = tf_im_to_eef * tf_eef_to_mesh;
426  tf2::Transform tf_root_to_mesh_new = tf_root_to_im * tf_im_to_mesh;
427  tf2::toMsg(tf_root_to_mesh_new, marker.pose);
428  // - - - - - - - - - - - - - - - - - - - - - - - - - -
429  m_control.markers.push_back(marker);
430  }
431 
432  im.controls.push_back(m_control);
433 }
434 
435 static inline std::string getMarkerName(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef)
436 {
437  return "EE:" + handler->getName() + "_" + eef.parent_link;
438 }
439 
440 static inline std::string getMarkerName(const InteractionHandlerPtr& handler, const JointInteraction& vj)
441 {
442  return "JJ:" + handler->getName() + "_" + vj.connecting_link;
443 }
444 
445 static inline std::string getMarkerName(const InteractionHandlerPtr& handler, const GenericInteraction& g)
446 {
447  return "GG:" + handler->getName() + "_" + g.marker_name_suffix;
448 }
449 
450 void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handler, const double marker_scale)
451 {
452  // 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
453  std::vector<visualization_msgs::msg::InteractiveMarker> ims;
454  {
455  std::unique_lock<std::mutex> ulock(marker_access_lock_);
456  moveit::core::RobotStateConstPtr s = handler->getState();
457 
458  for (std::size_t i = 0; i < active_generic_.size(); ++i)
459  {
460  visualization_msgs::msg::InteractiveMarker im;
461  if (active_generic_[i].construct_marker(*s, im))
462  {
463  im.name = getMarkerName(handler, active_generic_[i]);
464  shown_markers_[im.name] = i;
465  ims.push_back(im);
466  RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale);
467  }
468  }
469 
470  for (std::size_t i = 0; i < active_eef_.size(); ++i)
471  {
472  geometry_msgs::msg::PoseStamped pose;
473  geometry_msgs::msg::Pose control_to_eef_tf;
474  pose.header.frame_id = robot_model_->getModelFrame();
475  pose.header.stamp = node_->now();
476  computeMarkerPose(handler, active_eef_[i], *s, pose.pose, control_to_eef_tf);
477 
478  std::string marker_name = getMarkerName(handler, active_eef_[i]);
479  shown_markers_[marker_name] = i;
480 
481  // Determine interactive maker size
482  double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_eef_[i].size : marker_scale;
483 
484  visualization_msgs::msg::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
485  if (handler && handler->getControlsVisible())
486  {
487  if (active_eef_[i].interaction & InteractionStyle::POSITION_ARROWS)
488  addPositionControl(im, active_eef_[i].interaction & InteractionStyle::FIXED);
489  if (active_eef_[i].interaction & InteractionStyle::ORIENTATION_CIRCLES)
490  addOrientationControl(im, active_eef_[i].interaction & InteractionStyle::FIXED);
491  if (active_eef_[i].interaction & (InteractionStyle::POSITION_SPHERE | InteractionStyle::ORIENTATION_SPHERE))
492  {
493  std_msgs::msg::ColorRGBA color;
494  color.r = 0;
495  color.g = 1;
496  color.b = 1;
497  color.a = 0.5;
498  addViewPlaneControl(im, mscale * 0.25, color, active_eef_[i].interaction & InteractionStyle::POSITION_SPHERE,
499  active_eef_[i].interaction & InteractionStyle::ORIENTATION_SPHERE);
500  }
501  }
502  if (handler && handler->getMeshesVisible() &&
503  (active_eef_[i].interaction & (InteractionStyle::POSITION_EEF | InteractionStyle::ORIENTATION_EEF)))
504  addEndEffectorMarkers(handler, active_eef_[i], control_to_eef_tf, im,
505  active_eef_[i].interaction & InteractionStyle::POSITION_EEF,
506  active_eef_[i].interaction & InteractionStyle::ORIENTATION_EEF);
507  ims.push_back(im);
508  registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link);
509  RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale);
510  }
511  for (std::size_t i = 0; i < active_vj_.size(); ++i)
512  {
513  geometry_msgs::msg::PoseStamped pose;
514  pose.header.frame_id = robot_model_->getModelFrame();
515  pose.header.stamp = node_->now();
516  pose.pose = tf2::toMsg(s->getGlobalLinkTransform(active_vj_[i].connecting_link));
517  std::string marker_name = getMarkerName(handler, active_vj_[i]);
518  shown_markers_[marker_name] = i;
519 
520  // Determine interactive maker size
521  double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_vj_[i].size : marker_scale;
522 
523  visualization_msgs::msg::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
524  if (handler && handler->getControlsVisible())
525  {
526  if (active_vj_[i].dof == 3) // planar joint
527  addPlanarXYControl(im, false);
528  else
529  add6DOFControl(im, false);
530  }
531  ims.push_back(im);
532  registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link);
533  RCLCPP_DEBUG(LOGGER, "Publishing interactive marker %s (size = %lf)", marker_name.c_str(), mscale);
534  }
535  handlers_[handler->getName()] = handler;
536  }
537 
538  // we do this while marker_access_lock_ is unlocked because the interactive marker server locks
539  // for most function calls, and maintains that lock while the feedback callback is running
540  // that can cause a deadlock if we were to run the loop below while marker_access_lock_ is locked
541  for (const visualization_msgs::msg::InteractiveMarker& im : ims)
542  {
543  int_marker_server_->insert(im);
544  int_marker_server_->setCallback(im.name,
545  [this](const auto& feedback) { processInteractiveMarkerFeedback(feedback); });
546 
547  // Add menu handler to all markers that this interaction handler creates.
548  if (std::shared_ptr<interactive_markers::MenuHandler> mh = handler->getMenuHandler())
549  mh->apply(*int_marker_server_, im.name);
550  }
551 }
552 
553 void RobotInteraction::registerMoveInteractiveMarkerTopic(const std::string& marker_name, const std::string& name)
554 {
555  std::stringstream ss;
556  ss << "/rviz/moveit/move_marker/";
557  ss << name;
558  int_marker_move_topics_.push_back(ss.str());
559  int_marker_names_.push_back(marker_name);
560 }
561 
563 {
564  if (enable)
565  {
566  std::unique_lock<std::mutex> ulock(marker_access_lock_);
567  if (int_marker_move_subscribers_.empty())
568  {
569  for (size_t i = 0; i < int_marker_move_topics_.size(); ++i)
570  {
571  std::string topic_name = int_marker_move_topics_[i];
572  std::string marker_name = int_marker_names_[i];
573  std::function<void(const geometry_msgs::msg::PoseStamped::SharedPtr)> subscription_callback =
574  [this, marker_name](const geometry_msgs::msg::PoseStamped::SharedPtr& msg) {
575  moveInteractiveMarker(marker_name, *msg);
576  };
577  auto subscription = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
578  topic_name, rclcpp::SystemDefaultsQoS(), subscription_callback);
579  int_marker_move_subscribers_.push_back(subscription);
580  }
581  }
582  }
583  else
584  {
585  std::unique_lock<std::mutex> ulock(marker_access_lock_);
586  int_marker_move_subscribers_.clear();
587  }
588 }
589 
590 void RobotInteraction::computeMarkerPose(const InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
591  const moveit::core::RobotState& robot_state, geometry_msgs::msg::Pose& pose,
592  geometry_msgs::msg::Pose& control_to_eef_tf) const
593 {
594  // Need to allow for control pose offsets
595  tf2::Transform tf_root_to_link, tf_root_to_control;
596  tf2::fromMsg(tf2::toMsg(robot_state.getGlobalLinkTransform(eef.parent_link)), tf_root_to_link);
597 
598  geometry_msgs::msg::Pose msg_link_to_control;
599  if (handler->getPoseOffset(eef, msg_link_to_control))
600  {
601  tf2::Transform tf_link_to_control;
602  tf2::fromMsg(msg_link_to_control, tf_link_to_control);
603 
604  tf_root_to_control = tf_root_to_link * tf_link_to_control;
605  tf2::toMsg(tf_link_to_control.inverse(), control_to_eef_tf);
606  }
607  else
608  {
609  tf_root_to_control = tf_root_to_link;
610  control_to_eef_tf.orientation.x = 0.0;
611  control_to_eef_tf.orientation.y = 0.0;
612  control_to_eef_tf.orientation.z = 0.0;
613  control_to_eef_tf.orientation.w = 1.0;
614  }
615 
616  tf2::toMsg(tf_root_to_control, pose);
617 }
618 
619 void RobotInteraction::updateInteractiveMarkers(const InteractionHandlerPtr& handler)
620 {
621  std::string root_link;
622  std::map<std::string, geometry_msgs::msg::Pose> pose_updates;
623  {
624  std::unique_lock<std::mutex> ulock(marker_access_lock_);
625 
626  moveit::core::RobotStateConstPtr s = handler->getState();
627  root_link = s->getRobotModel()->getModelFrame();
628 
629  for (const EndEffectorInteraction& eef : active_eef_)
630  {
631  std::string marker_name = getMarkerName(handler, eef);
632  geometry_msgs::msg::Pose control_to_eef_tf;
633  computeMarkerPose(handler, eef, *s, pose_updates[marker_name], control_to_eef_tf);
634  }
635 
636  for (JointInteraction& vj : active_vj_)
637  {
638  std::string marker_name = getMarkerName(handler, vj);
639  pose_updates[marker_name] = tf2::toMsg(s->getGlobalLinkTransform(vj.connecting_link));
640  }
641 
642  for (GenericInteraction& gi : active_generic_)
643  {
644  std::string marker_name = getMarkerName(handler, gi);
645  geometry_msgs::msg::Pose pose;
646  if (gi.update_pose && gi.update_pose(*s, pose))
647  pose_updates[marker_name] = pose;
648  }
649  }
650 
651  std_msgs::msg::Header header;
652  header.frame_id = root_link; // marker poses are give w.r.t. root frame
653  for (std::map<std::string, geometry_msgs::msg::Pose>::const_iterator it = pose_updates.begin();
654  it != pose_updates.end(); ++it)
655  int_marker_server_->setPose(it->first, it->second, header);
656  int_marker_server_->applyChanges();
657 }
658 
660 {
661  // the server locks internally, so we need not worry about locking
662  int_marker_server_->applyChanges();
663 }
664 
665 bool RobotInteraction::showingMarkers(const InteractionHandlerPtr& handler)
666 {
667  std::unique_lock<std::mutex> ulock(marker_access_lock_);
668 
669  for (const EndEffectorInteraction& eef : active_eef_)
670  if (shown_markers_.find(getMarkerName(handler, eef)) == shown_markers_.end())
671  return false;
672  for (const JointInteraction& vj : active_vj_)
673  if (shown_markers_.find(getMarkerName(handler, vj)) == shown_markers_.end())
674  return false;
675  for (const GenericInteraction& gi : active_generic_)
676  if (shown_markers_.find(getMarkerName(handler, gi)) == shown_markers_.end())
677  return false;
678  return true;
679 }
680 
681 void RobotInteraction::moveInteractiveMarker(const std::string& name, const geometry_msgs::msg::PoseStamped& msg)
682 {
683  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(name);
684  if (it != shown_markers_.end())
685  {
686  auto feedback = std::make_shared<visualization_msgs::msg::InteractiveMarkerFeedback>();
687  feedback->header = msg.header;
688  feedback->marker_name = name;
689  feedback->pose = msg.pose;
690  feedback->event_type = visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE;
691  processInteractiveMarkerFeedback(feedback);
692  {
693  std::unique_lock<std::mutex> ulock(marker_access_lock_);
694  int_marker_server_->setPose(name, msg.pose, msg.header); // move the interactive marker
695  int_marker_server_->applyChanges();
696  }
697  }
698 }
699 
700 void RobotInteraction::processInteractiveMarkerFeedback(
701  const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback)
702 {
703  // perform some validity checks
704  std::unique_lock<std::mutex> ulock(marker_access_lock_);
705  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
706  if (it == shown_markers_.end())
707  {
708  RCLCPP_ERROR(LOGGER, "Unknown marker name: '%s' (not published by RobotInteraction class)",
709  feedback->marker_name.c_str());
710  return;
711  }
712 
713  std::size_t u = feedback->marker_name.find_first_of('_');
714  if (u == std::string::npos || u < 4)
715  {
716  RCLCPP_ERROR(LOGGER, "Invalid marker name: '%s'", feedback->marker_name.c_str());
717  return;
718  }
719 
720  feedback_map_[feedback->marker_name] = feedback;
721  new_feedback_condition_.notify_all();
722 }
723 
724 void RobotInteraction::processingThread()
725 {
726  std::unique_lock<std::mutex> ulock(marker_access_lock_);
727 
728  while (run_processing_thread_ && rclcpp::ok())
729  {
730  while (feedback_map_.empty() && run_processing_thread_ && rclcpp::ok())
731  new_feedback_condition_.wait(ulock);
732 
733  while (!feedback_map_.empty() && rclcpp::ok())
734  {
735  auto feedback = feedback_map_.begin()->second;
736  feedback_map_.erase(feedback_map_.begin());
737  RCLCPP_DEBUG(LOGGER, "Processing feedback from map for marker [%s]", feedback->marker_name.c_str());
738 
739  std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
740  if (it == shown_markers_.end())
741  {
742  RCLCPP_ERROR(LOGGER,
743  "Unknown marker name: '%s' (not published by RobotInteraction class) "
744  "(should never have ended up in the feedback_map!)",
745  feedback->marker_name.c_str());
746  continue;
747  }
748  std::size_t u = feedback->marker_name.find_first_of('_');
749  if (u == std::string::npos || u < 4)
750  {
751  RCLCPP_ERROR(LOGGER, "Invalid marker name: '%s' (should never have ended up in the feedback_map!)",
752  feedback->marker_name.c_str());
753  continue;
754  }
755  std::string marker_class = feedback->marker_name.substr(0, 2);
756  std::string handler_name = feedback->marker_name.substr(3, u - 3); // skip the ":"
757  std::map<std::string, InteractionHandlerPtr>::const_iterator jt = handlers_.find(handler_name);
758  if (jt == handlers_.end())
759  {
760  RCLCPP_ERROR(LOGGER, "Interactive Marker Handler '%s' is not known.", handler_name.c_str());
761  continue;
762  }
763 
764  // we put this in a try-catch because user specified callbacks may be triggered
765  try
766  {
767  if (marker_class == "EE")
768  {
769  // make a copy of the data, so we do not lose it while we are unlocked
770  EndEffectorInteraction eef = active_eef_[it->second];
771  InteractionHandlerPtr ih = jt->second;
772  marker_access_lock_.unlock();
773  try
774  {
775  ih->handleEndEffector(eef, feedback);
776  }
777  catch (std::exception& ex)
778  {
779  RCLCPP_ERROR(LOGGER, "Exception caught while handling end-effector update: %s", ex.what());
780  }
781  marker_access_lock_.lock();
782  }
783  else if (marker_class == "JJ")
784  {
785  // make a copy of the data, so we do not lose it while we are unlocked
786  JointInteraction vj = active_vj_[it->second];
787  InteractionHandlerPtr ih = jt->second;
788  marker_access_lock_.unlock();
789  try
790  {
791  ih->handleJoint(vj, feedback);
792  }
793  catch (std::exception& ex)
794  {
795  RCLCPP_ERROR(LOGGER, "Exception caught while handling joint update: %s", ex.what());
796  }
797  marker_access_lock_.lock();
798  }
799  else if (marker_class == "GG")
800  {
801  InteractionHandlerPtr ih = jt->second;
802  GenericInteraction g = active_generic_[it->second];
803  marker_access_lock_.unlock();
804  try
805  {
806  ih->handleGeneric(g, feedback);
807  }
808  catch (std::exception& ex)
809  {
810  RCLCPP_ERROR(LOGGER, "Exception caught while handling joint update: %s", ex.what());
811  }
812  marker_access_lock_.lock();
813  }
814  else
815  RCLCPP_ERROR(LOGGER, "Unknown marker class ('%s') for marker '%s'", marker_class.c_str(),
816  feedback->marker_name.c_str());
817  }
818  catch (std::exception& ex)
819  {
820  RCLCPP_ERROR(LOGGER, "Exception caught while processing event: %s", ex.what());
821  }
822  }
823  }
824 }
825 } // namespace robot_interaction
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
JointType getType() const
Get the type of joint.
Definition: joint_model.h:151
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes tha...
Definition: link_model.h:186
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Definition: link_model.h:117
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1339
void updateInteractiveMarkers(const InteractionHandlerPtr &handler)
void decideActiveComponents(const std::string &group)
RobotInteraction(const moveit::core::RobotModelConstPtr &robot_model, const rclcpp::Node::SharedPtr &node, const std::string &ns="")
bool showingMarkers(const InteractionHandlerPtr &handler)
void addActiveComponent(const InteractiveMarkerConstructorFn &construct, const ProcessFeedbackFn &process, const InteractiveMarkerUpdateFn &update=InteractiveMarkerUpdateFn(), const std::string &name="")
void addInteractiveMarkers(const InteractionHandlerPtr &handler, const double marker_scale=0.0)
static const std::string INTERACTIVE_MARKER_TOPIC
The topic name on which the internal Interactive Marker Server operates.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
void addViewPlaneControl(visualization_msgs::msg::InteractiveMarker &int_marker, double radius, const std_msgs::msg::ColorRGBA &color, bool position=true, bool orientation=true)
void add6DOFControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
std::function< bool(moveit::core::RobotState &state, const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback)> ProcessFeedbackFn
Definition: interaction.h:103
std::function< bool(const moveit::core::RobotState &, geometry_msgs::msg::Pose &)> InteractiveMarkerUpdateFn
Definition: interaction.h:114
std::function< bool(const moveit::core::RobotState &state, visualization_msgs::msg::InteractiveMarker &marker)> InteractiveMarkerConstructorFn
Definition: interaction.h:88
void addOrientationControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
visualization_msgs::msg::InteractiveMarker makeEmptyInteractiveMarker(const std::string &name, const geometry_msgs::msg::PoseStamped &stamped, double scale)
void addPlanarXYControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
void addPositionControl(visualization_msgs::msg::InteractiveMarker &int_marker, bool orientation_fixed=false)
name
Definition: setup.py:7
const rclcpp::Logger LOGGER
Definition: async_test.h:31
std::string parent_link
The name of the link in the parent group that connects to the end-effector.
Definition: interaction.h:145
InteractiveMarkerConstructorFn construct_marker
Definition: interaction.h:122
InteractiveMarkerUpdateFn update_pose
Definition: interaction.h:130