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>
54 #include <Eigen/Geometry>
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 };
65 const rclcpp::Node::SharedPtr& node,
const std::string& ns)
66 : robot_model_(robot_model), kinematic_options_map_(std::make_shared<
KinematicOptionsMap>())
70 int_marker_server_ =
new interactive_markers::InteractiveMarkerServer(topic_, node_);
73 run_processing_thread_ =
true;
74 processing_thread_ = std::make_unique<std::thread>([
this] { processingThread(); });
79 run_processing_thread_ =
false;
80 new_feedback_condition_.notify_all();
81 processing_thread_->join();
84 delete int_marker_server_;
94 decideActiveEndEffectors(
group, style);
95 decideActiveJoints(
group);
96 if (!
group.empty() && active_eef_.empty() && active_vj_.empty() && active_generic_.empty())
98 "No active joints or end effectors found for group '%s'. "
99 "Make sure that kinematics.yaml is loaded in this node's namespace.",
105 const std::string&
name)
107 std::unique_lock<std::mutex> ulock(marker_access_lock_);
114 active_generic_.push_back(g);
117 static const double DEFAULT_SCALE = 0.25;
118 double RobotInteraction::computeLinkMarkerSize(
const std::string& link)
127 Eigen::MatrixXd::Index max_index;
128 ext.maxCoeff(&max_index);
130 size = 1.01 * ext.norm();
142 return DEFAULT_SCALE;
148 double RobotInteraction::computeGroupMarkerSize(
const std::string&
group)
151 return DEFAULT_SCALE;
158 return DEFAULT_SCALE;
162 for (
const std::string& link : links)
170 Eigen::MatrixXd::Index max_index;
171 ext.maxCoeff(&max_index);
173 size = std::max(size, 1.01 * ext.norm());
179 return computeLinkMarkerSize(links[0]);
185 void RobotInteraction::decideActiveJoints(
const std::string&
group)
187 std::unique_lock<std::mutex> ulock(marker_access_lock_);
193 RCLCPP_DEBUG(LOGGER,
"Deciding active joints for group '%s'",
group.c_str());
195 const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
201 std::set<std::string> used;
205 default_state.setToDefaultValues();
206 std::vector<double> aabb;
207 default_state.computeAABB(aabb);
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())
213 if (joint.type_ ==
"planar" || joint.type_ ==
"floating")
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")
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);
233 const std::vector<const moveit::core::JointModel*>& joints = jmg->
getJointModels();
238 used.find(joint->getName()) == used.end())
241 v.connecting_link = joint->getChildLinkModel()->getName();
242 if (joint->getParentLinkModel())
243 v.parent_frame = joint->getParentLinkModel()->getName();
244 v.joint_name = joint->getName();
250 v.size = computeGroupMarkerSize(
group);
251 active_vj_.push_back(v);
256 void RobotInteraction::decideActiveEndEffectors(
const std::string&
group)
263 std::unique_lock<std::mutex> ulock(marker_access_lock_);
269 RCLCPP_DEBUG(LOGGER,
"Deciding active end-effectors for group '%s'",
group.c_str());
271 const srdf::ModelConstSharedPtr& srdf = robot_model_->getSRDF();
276 RCLCPP_WARN(LOGGER,
"Unable to decide active end effector: no joint model group or no srdf model");
280 const std::vector<srdf::Model::EndEffector>& eefs = srdf->getEndEffectors();
281 const std::pair<moveit::core::JointModelGroup::KinematicsSolver, moveit::core::JointModelGroup::KinematicsSolverMap>&
285 bool found_eef{
false };
286 for (
const srdf::Model::EndEffector& eef : eefs)
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_))
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);
304 if (!found_eef && !single_group->getLinkModelNames().empty())
306 std::string last_link{ single_group->getLinkModelNames().back() };
308 if (single_group->canSetStateFromIK(last_link))
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);
323 add_active_end_effectors_for_single_group(jmg);
326 else if (!smap.second.empty())
328 for (
const std::pair<const moveit::core::JointModelGroup* const, moveit::core::JointModelGroup::KinematicsSolver>&
330 add_active_end_effectors_for_single_group(it.first);
334 for (EndEffectorInteraction& eef : active_eef_)
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);
343 if (active_eef_.size() == 1)
344 active_eef_[0].size *= 1.5;
349 std::unique_lock<std::mutex> ulock(marker_access_lock_);
352 active_generic_.clear();
353 clearInteractiveMarkersUnsafe();
359 std::unique_lock<std::mutex> ulock(marker_access_lock_);
360 clearInteractiveMarkersUnsafe();
363 void RobotInteraction::clearInteractiveMarkersUnsafe()
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();
373 void RobotInteraction::addEndEffectorMarkers(
const InteractionHandlerPtr& handler,
const EndEffectorInteraction& eef,
374 visualization_msgs::msg::InteractiveMarker& im,
bool position,
377 geometry_msgs::msg::Pose pose;
378 pose.orientation.w = 1;
379 addEndEffectorMarkers(handler, eef, pose, im, position, orientation);
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,
387 if (eef.parent_group == eef.eef_group || !robot_model_->hasLinkModel(eef.parent_link))
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;
397 m_control.interaction_mode = m_control.MOVE_3D;
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];
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);
415 for (visualization_msgs::msg::Marker& marker : marker_array.markers)
417 marker.header = im.header;
418 marker.mesh_use_embedded_materials = !marker.mesh_resource.empty();
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);
429 m_control.markers.push_back(marker);
432 im.controls.push_back(m_control);
435 static inline std::string getMarkerName(
const InteractionHandlerPtr& handler,
const EndEffectorInteraction& eef)
437 return "EE:" + handler->getName() +
"_" + eef.parent_link;
440 static inline std::string getMarkerName(
const InteractionHandlerPtr& handler,
const JointInteraction& vj)
442 return "JJ:" + handler->getName() +
"_" + vj.connecting_link;
445 static inline std::string getMarkerName(
const InteractionHandlerPtr& handler,
const GenericInteraction& g)
447 return "GG:" + handler->getName() +
"_" + g.marker_name_suffix;
453 std::vector<visualization_msgs::msg::InteractiveMarker> ims;
455 std::unique_lock<std::mutex> ulock(marker_access_lock_);
456 moveit::core::RobotStateConstPtr s = handler->getState();
458 for (std::size_t i = 0; i < active_generic_.size(); ++i)
460 visualization_msgs::msg::InteractiveMarker im;
461 if (active_generic_[i].construct_marker(*s, im))
463 im.name = getMarkerName(handler, active_generic_[i]);
464 shown_markers_[im.name] = i;
466 RCLCPP_DEBUG(LOGGER,
"Publishing interactive marker %s (size = %lf)", im.name.c_str(), im.scale);
470 for (std::size_t i = 0; i < active_eef_.size(); ++i)
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);
478 std::string marker_name = getMarkerName(handler, active_eef_[i]);
479 shown_markers_[marker_name] = i;
482 double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_eef_[i].size : marker_scale;
485 if (handler && handler->getControlsVisible())
493 std_msgs::msg::ColorRGBA color;
502 if (handler && handler->getMeshesVisible() &&
504 addEndEffectorMarkers(handler, active_eef_[i], control_to_eef_tf, 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);
511 for (std::size_t i = 0; i < active_vj_.size(); ++i)
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;
521 double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_vj_[i].size : marker_scale;
524 if (handler && handler->getControlsVisible())
526 if (active_vj_[i].dof == 3)
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);
535 handlers_[handler->getName()] = handler;
541 for (
const visualization_msgs::msg::InteractiveMarker& im : ims)
543 int_marker_server_->insert(im);
544 int_marker_server_->setCallback(im.name,
545 [
this](
const auto& feedback) { processInteractiveMarkerFeedback(feedback); });
548 if (std::shared_ptr<interactive_markers::MenuHandler> mh = handler->getMenuHandler())
549 mh->apply(*int_marker_server_, im.name);
553 void RobotInteraction::registerMoveInteractiveMarkerTopic(
const std::string& marker_name,
const std::string&
name)
555 std::stringstream ss;
556 ss <<
"/rviz/moveit/move_marker/";
558 int_marker_move_topics_.push_back(ss.str());
559 int_marker_names_.push_back(marker_name);
566 std::unique_lock<std::mutex> ulock(marker_access_lock_);
567 if (int_marker_move_subscribers_.empty())
569 for (
size_t i = 0; i < int_marker_move_topics_.size(); ++i)
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);
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);
585 std::unique_lock<std::mutex> ulock(marker_access_lock_);
586 int_marker_move_subscribers_.clear();
590 void RobotInteraction::computeMarkerPose(
const InteractionHandlerPtr& handler,
const EndEffectorInteraction& eef,
592 geometry_msgs::msg::Pose& control_to_eef_tf)
const
595 tf2::Transform tf_root_to_link, tf_root_to_control;
598 geometry_msgs::msg::Pose msg_link_to_control;
599 if (handler->getPoseOffset(eef, msg_link_to_control))
601 tf2::Transform tf_link_to_control;
602 tf2::fromMsg(msg_link_to_control, tf_link_to_control);
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);
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;
616 tf2::toMsg(tf_root_to_control, pose);
621 std::string root_link;
622 std::map<std::string, geometry_msgs::msg::Pose> pose_updates;
624 std::unique_lock<std::mutex> ulock(marker_access_lock_);
626 moveit::core::RobotStateConstPtr s = handler->getState();
627 root_link = s->getRobotModel()->getModelFrame();
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);
638 std::string marker_name = getMarkerName(handler, vj);
639 pose_updates[marker_name] = tf2::toMsg(s->getGlobalLinkTransform(vj.connecting_link));
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;
651 std_msgs::msg::Header header;
652 header.frame_id = root_link;
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();
662 int_marker_server_->applyChanges();
667 std::unique_lock<std::mutex> ulock(marker_access_lock_);
670 if (shown_markers_.find(getMarkerName(handler, eef)) == shown_markers_.end())
673 if (shown_markers_.find(getMarkerName(handler, vj)) == shown_markers_.end())
676 if (shown_markers_.find(getMarkerName(handler, gi)) == shown_markers_.end())
681 void RobotInteraction::moveInteractiveMarker(
const std::string&
name,
const geometry_msgs::msg::PoseStamped& msg)
683 std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(
name);
684 if (it != shown_markers_.end())
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);
693 std::unique_lock<std::mutex> ulock(marker_access_lock_);
694 int_marker_server_->setPose(
name, msg.pose, msg.header);
695 int_marker_server_->applyChanges();
700 void RobotInteraction::processInteractiveMarkerFeedback(
701 const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback)
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())
708 RCLCPP_ERROR(LOGGER,
"Unknown marker name: '%s' (not published by RobotInteraction class)",
709 feedback->marker_name.c_str());
713 std::size_t u = feedback->marker_name.find_first_of(
'_');
714 if (u == std::string::npos || u < 4)
716 RCLCPP_ERROR(LOGGER,
"Invalid marker name: '%s'", feedback->marker_name.c_str());
720 feedback_map_[feedback->marker_name] = feedback;
721 new_feedback_condition_.notify_all();
724 void RobotInteraction::processingThread()
726 std::unique_lock<std::mutex> ulock(marker_access_lock_);
728 while (run_processing_thread_ && rclcpp::ok())
730 while (feedback_map_.empty() && run_processing_thread_ && rclcpp::ok())
731 new_feedback_condition_.wait(ulock);
733 while (!feedback_map_.empty() && rclcpp::ok())
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());
739 std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
740 if (it == shown_markers_.end())
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());
748 std::size_t u = feedback->marker_name.find_first_of(
'_');
749 if (u == std::string::npos || u < 4)
751 RCLCPP_ERROR(LOGGER,
"Invalid marker name: '%s' (should never have ended up in the feedback_map!)",
752 feedback->marker_name.c_str());
755 std::string marker_class = feedback->marker_name.substr(0, 2);
756 std::string handler_name = feedback->marker_name.substr(3, u - 3);
757 std::map<std::string, InteractionHandlerPtr>::const_iterator jt = handlers_.find(handler_name);
758 if (jt == handlers_.end())
760 RCLCPP_ERROR(LOGGER,
"Interactive Marker Handler '%s' is not known.", handler_name.c_str());
767 if (marker_class ==
"EE")
770 EndEffectorInteraction eef = active_eef_[it->second];
771 InteractionHandlerPtr ih = jt->second;
772 marker_access_lock_.unlock();
775 ih->handleEndEffector(eef, feedback);
777 catch (std::exception& ex)
779 RCLCPP_ERROR(LOGGER,
"Exception caught while handling end-effector update: %s", ex.what());
781 marker_access_lock_.lock();
783 else if (marker_class ==
"JJ")
786 JointInteraction vj = active_vj_[it->second];
787 InteractionHandlerPtr ih = jt->second;
788 marker_access_lock_.unlock();
791 ih->handleJoint(vj, feedback);
793 catch (std::exception& ex)
795 RCLCPP_ERROR(LOGGER,
"Exception caught while handling joint update: %s", ex.what());
797 marker_access_lock_.lock();
799 else if (marker_class ==
"GG")
801 InteractionHandlerPtr ih = jt->second;
802 GenericInteraction g = active_generic_[it->second];
803 marker_access_lock_.unlock();
806 ih->handleGeneric(g, feedback);
808 catch (std::exception& ex)
810 RCLCPP_ERROR(LOGGER,
"Exception caught while handling joint update: %s", ex.what());
812 marker_access_lock_.lock();
815 RCLCPP_ERROR(LOGGER,
"Unknown marker class ('%s') for marker '%s'", marker_class.c_str(),
816 feedback->marker_name.c_str());
818 catch (std::exception& ex)
820 RCLCPP_ERROR(LOGGER,
"Exception caught while processing event: %s", ex.what());
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....
JointType getType() const
Get the type of joint.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes tha...
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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...
void publishInteractiveMarkers()
void clearInteractiveMarkers()
void updateInteractiveMarkers(const InteractionHandlerPtr &handler)
void decideActiveComponents(const std::string &group)
void toggleMoveInteractiveMarkerTopic(bool enable)
virtual ~RobotInteraction()
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
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
std::function< bool(const moveit::core::RobotState &, geometry_msgs::msg::Pose &)> InteractiveMarkerUpdateFn
std::function< bool(const moveit::core::RobotState &state, visualization_msgs::msg::InteractiveMarker &marker)> InteractiveMarkerConstructorFn
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)
const rclcpp::Logger LOGGER
std::string parent_link
The name of the link in the parent group that connects to the end-effector.
std::string marker_name_suffix
ProcessFeedbackFn process_feedback
InteractiveMarkerConstructorFn construct_marker
InteractiveMarkerUpdateFn update_pose