robot_interaction.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012-2013, Willow Garage, Inc.
00005  *  Copyright (c) 2013, Ioan A. Sucan
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan, Adam Leeper */
00037 
00038 #include "moveit/robot_interaction/robot_interaction.h"
00039 #include <moveit/robot_interaction/interaction_handler.h>
00040 #include <moveit/robot_interaction/interactive_marker_helpers.h>
00041 #include <moveit/robot_interaction/kinematic_options_map.h>
00042 #include <moveit/transforms/transforms.h>
00043 #include <interactive_markers/interactive_marker_server.h>
00044 #include <interactive_markers/menu_handler.h>
00045 #include <eigen_conversions/eigen_msg.h>
00046 #include <tf_conversions/tf_eigen.h>
00047 #include <boost/lexical_cast.hpp>
00048 #include <boost/math/constants/constants.hpp>
00049 #include <boost/algorithm/string.hpp>
00050 
00051 #include <algorithm>
00052 #include <limits>
00053 
00054 #include <Eigen/Core>
00055 #include <Eigen/Geometry>
00056 
00057 namespace robot_interaction
00058 {
00059 static const float END_EFFECTOR_UNREACHABLE_COLOR[4] = { 1.0, 0.3, 0.3, 1.0 };
00060 static const float END_EFFECTOR_REACHABLE_COLOR[4] = { 0.2, 1.0, 0.2, 1.0 };
00061 
00062 const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interaction_interactive_marker_topic";
00063 
00064 RobotInteraction::RobotInteraction(const robot_model::RobotModelConstPtr& robot_model, const std::string& ns)
00065   : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap)
00066 {
00067   topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC;
00068   int_marker_server_ = new interactive_markers::InteractiveMarkerServer(topic_);
00069 
00070   // spin a thread that will process feedback events
00071   run_processing_thread_ = true;
00072   processing_thread_.reset(new boost::thread(boost::bind(&RobotInteraction::processingThread, this)));
00073 }
00074 
00075 RobotInteraction::~RobotInteraction()
00076 {
00077   run_processing_thread_ = false;
00078   new_feedback_condition_.notify_all();
00079   processing_thread_->join();
00080 
00081   clear();
00082   delete int_marker_server_;
00083 }
00084 
00085 void RobotInteraction::decideActiveComponents(const std::string& group)
00086 {
00087   decideActiveComponents(group, InteractionStyle::SIX_DOF);
00088 }
00089 
00090 void RobotInteraction::decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style)
00091 {
00092   decideActiveEndEffectors(group, style);
00093   decideActiveJoints(group);
00094   if (active_eef_.empty() && active_vj_.empty() && active_generic_.empty())
00095     ROS_INFO_NAMED("robot_interaction", "No active joints or end effectors found for group '%s'. "
00096                                         "Make sure you have defined an end effector in your SRDF file and that "
00097                                         "kinematics.yaml is loaded in this node's namespace.",
00098                    group.c_str());
00099 }
00100 
00101 void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn& construct,
00102                                           const ProcessFeedbackFn& process, const InteractiveMarkerUpdateFn& update,
00103                                           const std::string& name)
00104 {
00105   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00106   GenericInteraction g;
00107   g.construct_marker = construct;
00108   g.update_pose = update;
00109   g.process_feedback = process;
00110   // compute the suffix that will be added to the generated markers
00111   g.marker_name_suffix = "_" + name + "_" + boost::lexical_cast<std::string>(active_generic_.size());
00112   active_generic_.push_back(g);
00113 }
00114 
00115 static const double DEFAULT_SCALE = 0.25;
00116 double RobotInteraction::computeLinkMarkerSize(const std::string& link)
00117 {
00118   const robot_model::LinkModel* lm = robot_model_->getLinkModel(link);
00119   double size = 0;
00120 
00121   while (lm)
00122   {
00123     Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin();
00124     // drop largest extension and take norm of two remaining
00125     Eigen::MatrixXd::Index maxIndex;
00126     ext.maxCoeff(&maxIndex);
00127     ext[maxIndex] = 0;
00128     size = 1.01 * ext.norm();
00129     if (size > 0)
00130       break;  // break, if a non-empty shape was found
00131 
00132     // process kinematic chain upwards (but only following fixed joints)
00133     // to find a link with some non-empty shape (to ignore virtual links)
00134     if (lm->getParentJointModel()->getType() == robot_model::JointModel::FIXED)
00135       lm = lm->getParentLinkModel();
00136     else
00137       lm = 0;
00138   }
00139   if (!lm)
00140     return DEFAULT_SCALE;  // no link with non-zero shape extends found
00141 
00142   // the marker sphere will be half the size, so double the size here
00143   return 2. * size;
00144 }
00145 
00146 double RobotInteraction::computeGroupMarkerSize(const std::string& group)
00147 {
00148   if (group.empty())
00149     return DEFAULT_SCALE;
00150   const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00151   if (!jmg)
00152     return 0.0;
00153 
00154   const std::vector<std::string>& links = jmg->getLinkModelNames();
00155   if (links.empty())
00156     return DEFAULT_SCALE;
00157 
00158   // compute the aabb of the links that make up the group
00159   double size = 0;
00160   for (std::size_t i = 0; i < links.size(); ++i)
00161   {
00162     const robot_model::LinkModel* lm = robot_model_->getLinkModel(links[i]);
00163     if (!lm)
00164       continue;
00165     Eigen::Vector3d ext = lm->getShapeExtentsAtOrigin();
00166 
00167     // drop largest extension and take norm of two remaining
00168     Eigen::MatrixXd::Index maxIndex;
00169     ext.maxCoeff(&maxIndex);
00170     ext[maxIndex] = 0;
00171     size = std::max(size, 1.01 * ext.norm());
00172   }
00173 
00174   // if size is zero, all links have empty shapes and are placed at same position
00175   // in this case, fall back to link marker size
00176   if (size == 0)
00177     return computeLinkMarkerSize(links[0]);
00178 
00179   // the marker sphere will be half the size, so double the size here
00180   return 2. * size;
00181 }
00182 
00183 void RobotInteraction::decideActiveJoints(const std::string& group)
00184 {
00185   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00186   active_vj_.clear();
00187 
00188   ROS_DEBUG_NAMED("robot_interaction", "Deciding active joints for group '%s'", group.c_str());
00189 
00190   if (group.empty())
00191     return;
00192 
00193   const boost::shared_ptr<const srdf::Model>& srdf = robot_model_->getSRDF();
00194   const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00195 
00196   if (!jmg || !srdf)
00197     return;
00198 
00199   std::set<std::string> used;
00200   if (jmg->hasJointModel(robot_model_->getRootJointName()))
00201   {
00202     robot_state::RobotState default_state(robot_model_);
00203     default_state.setToDefaultValues();
00204     std::vector<double> aabb;
00205     default_state.computeAABB(aabb);
00206 
00207     const std::vector<srdf::Model::VirtualJoint>& vj = srdf->getVirtualJoints();
00208     for (std::size_t i = 0; i < vj.size(); ++i)
00209       if (vj[i].name_ == robot_model_->getRootJointName())
00210       {
00211         if (vj[i].type_ == "planar" || vj[i].type_ == "floating")
00212         {
00213           JointInteraction v;
00214           v.connecting_link = vj[i].child_link_;
00215           v.parent_frame = vj[i].parent_frame_;
00216           if (!v.parent_frame.empty() && v.parent_frame[0] == '/')
00217             v.parent_frame = v.parent_frame.substr(1);
00218           v.joint_name = vj[i].name_;
00219           if (vj[i].type_ == "planar")
00220             v.dof = 3;
00221           else
00222             v.dof = 6;
00223           // take the max of the X, Y, Z extent
00224           v.size = std::max(std::max(aabb[1] - aabb[0], aabb[3] - aabb[2]), aabb[5] - aabb[4]);
00225           active_vj_.push_back(v);
00226           used.insert(v.joint_name);
00227         }
00228       }
00229   }
00230 
00231   const std::vector<const robot_model::JointModel*>& joints = jmg->getJointModels();
00232   for (std::size_t i = 0; i < joints.size(); ++i)
00233   {
00234     if ((joints[i]->getType() == robot_model::JointModel::PLANAR ||
00235          joints[i]->getType() == robot_model::JointModel::FLOATING) &&
00236         used.find(joints[i]->getName()) == used.end())
00237     {
00238       JointInteraction v;
00239       v.connecting_link = joints[i]->getChildLinkModel()->getName();
00240       if (joints[i]->getParentLinkModel())
00241         v.parent_frame = joints[i]->getParentLinkModel()->getName();
00242       v.joint_name = joints[i]->getName();
00243       if (joints[i]->getType() == robot_model::JointModel::PLANAR)
00244         v.dof = 3;
00245       else
00246         v.dof = 6;
00247       // take the max of the X, Y, Z extent
00248       v.size = computeGroupMarkerSize(group);
00249       active_vj_.push_back(v);
00250     }
00251   }
00252 }
00253 
00254 void RobotInteraction::decideActiveEndEffectors(const std::string& group)
00255 {
00256   decideActiveEndEffectors(group, InteractionStyle::SIX_DOF);
00257 }
00258 
00259 void RobotInteraction::decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style)
00260 {
00261   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00262   active_eef_.clear();
00263 
00264   ROS_DEBUG_NAMED("robot_interaction", "Deciding active end-effectors for group '%s'", group.c_str());
00265 
00266   if (group.empty())
00267     return;
00268 
00269   const boost::shared_ptr<const srdf::Model>& srdf = robot_model_->getSRDF();
00270   const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00271 
00272   if (!jmg || !srdf)
00273   {
00274     ROS_WARN_NAMED("robot_interaction", "Unable to decide active end effector: no joint model group or no srdf model");
00275     return;
00276   }
00277 
00278   const std::vector<srdf::Model::EndEffector>& eef = srdf->getEndEffectors();
00279   const std::pair<robot_model::JointModelGroup::KinematicsSolver, robot_model::JointModelGroup::KinematicsSolverMap>&
00280       smap = jmg->getGroupKinematics();
00281 
00282   // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group
00283   if (smap.first)
00284   {
00285     if (eef.empty() && !jmg->getLinkModelNames().empty())
00286     {
00287       // No end effectors.  Use last link in group as the "end effector".
00288       EndEffectorInteraction ee;
00289       ee.parent_group = group;
00290       ee.parent_link = jmg->getLinkModelNames().back();
00291       ee.eef_group = group;
00292       ee.interaction = style;
00293       active_eef_.push_back(ee);
00294     }
00295     else
00296     {
00297       for (std::size_t i = 0; i < eef.size(); ++i)
00298         if ((jmg->hasLinkModel(eef[i].parent_link_) || jmg->getName() == eef[i].parent_group_) &&
00299             jmg->canSetStateFromIK(eef[i].parent_link_))
00300         {
00301           // We found an end-effector whose parent is the group.
00302           EndEffectorInteraction ee;
00303           ee.parent_group = group;
00304           ee.parent_link = eef[i].parent_link_;
00305           ee.eef_group = eef[i].component_group_;
00306           ee.interaction = style;
00307           active_eef_.push_back(ee);
00308         }
00309     }
00310   }
00311   else if (!smap.second.empty())
00312   {
00313     for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = smap.second.begin();
00314          it != smap.second.end(); ++it)
00315     {
00316       for (std::size_t i = 0; i < eef.size(); ++i)
00317       {
00318         if ((it->first->hasLinkModel(eef[i].parent_link_) || jmg->getName() == eef[i].parent_group_) &&
00319             it->first->canSetStateFromIK(eef[i].parent_link_))
00320         {
00321           // We found an end-effector whose parent is a subgroup of the group.  (May be more than one)
00322           EndEffectorInteraction ee;
00323           ee.parent_group = it->first->getName();
00324           ee.parent_link = eef[i].parent_link_;
00325           ee.eef_group = eef[i].component_group_;
00326           ee.interaction = style;
00327           active_eef_.push_back(ee);
00328           break;
00329         }
00330       }
00331     }
00332   }
00333 
00334   for (std::size_t i = 0; i < active_eef_.size(); ++i)
00335   {
00336     // if we have a separate group for the eef, we compute the scale based on it;
00337     // otherwise, we use the size of the parent_link
00338     active_eef_[i].size = active_eef_[i].eef_group == active_eef_[i].parent_group ?
00339                               computeLinkMarkerSize(active_eef_[i].parent_link) :
00340                               computeGroupMarkerSize(active_eef_[i].eef_group);
00341     ROS_DEBUG_NAMED("robot_interaction", "Found active end-effector '%s', of scale %lf",
00342                     active_eef_[i].eef_group.c_str(), active_eef_[i].size);
00343   }
00344   // if there is only a single end effector marker, we can safely use a larger marker
00345   if (active_eef_.size() == 1)
00346     active_eef_[0].size *= 1.5;
00347 }
00348 
00349 void RobotInteraction::clear()
00350 {
00351   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00352   active_eef_.clear();
00353   active_vj_.clear();
00354   active_generic_.clear();
00355   clearInteractiveMarkersUnsafe();
00356   publishInteractiveMarkers();
00357 }
00358 
00359 void RobotInteraction::clearInteractiveMarkers()
00360 {
00361   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00362   clearInteractiveMarkersUnsafe();
00363 }
00364 
00365 void RobotInteraction::clearInteractiveMarkersUnsafe()
00366 {
00367   handlers_.clear();
00368   shown_markers_.clear();
00369   int_marker_move_subscribers_.clear();
00370   int_marker_move_topics_.clear();
00371   int_marker_names_.clear();
00372   int_marker_server_->clear();
00373 }
00374 
00375 void RobotInteraction::addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00376                                              const EndEffectorInteraction& eef,
00377                                              visualization_msgs::InteractiveMarker& im, bool position, bool orientation)
00378 {
00379   geometry_msgs::Pose pose;
00380   pose.orientation.w = 1;
00381   addEndEffectorMarkers(handler, eef, pose, im, position, orientation);
00382 }
00383 
00384 void RobotInteraction::addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00385                                              const EndEffectorInteraction& eef, const geometry_msgs::Pose& im_to_eef,
00386                                              visualization_msgs::InteractiveMarker& im, bool position, bool orientation)
00387 {
00388   if (eef.parent_group == eef.eef_group || !robot_model_->hasLinkModel(eef.parent_link))
00389     return;
00390 
00391   visualization_msgs::InteractiveMarkerControl m_control;
00392   m_control.always_visible = false;
00393   if (position && orientation)
00394     m_control.interaction_mode = m_control.MOVE_ROTATE_3D;
00395   else if (orientation)
00396     m_control.interaction_mode = m_control.ROTATE_3D;
00397   else
00398     m_control.interaction_mode = m_control.MOVE_3D;
00399 
00400   std_msgs::ColorRGBA marker_color;
00401   const float* color = handler->inError(eef) ? END_EFFECTOR_UNREACHABLE_COLOR : END_EFFECTOR_REACHABLE_COLOR;
00402   marker_color.r = color[0];
00403   marker_color.g = color[1];
00404   marker_color.b = color[2];
00405   marker_color.a = color[3];
00406 
00407   robot_state::RobotStateConstPtr rstate = handler->getState();
00408   const std::vector<std::string>& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames();
00409   visualization_msgs::MarkerArray marker_array;
00410   rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, ros::Duration());
00411   tf::Pose tf_root_to_link;
00412   tf::poseEigenToTF(rstate->getGlobalLinkTransform(eef.parent_link), tf_root_to_link);
00413   // Release the ptr count on the kinematic state
00414   rstate.reset();
00415 
00416   for (std::size_t i = 0; i < marker_array.markers.size(); ++i)
00417   {
00418     marker_array.markers[i].header = im.header;
00419     marker_array.markers[i].mesh_use_embedded_materials = true;
00420     // - - - - - - Do some math for the offset - - - - - -
00421     tf::Pose tf_root_to_im, tf_root_to_mesh, tf_im_to_eef;
00422     tf::poseMsgToTF(im.pose, tf_root_to_im);
00423     tf::poseMsgToTF(marker_array.markers[i].pose, tf_root_to_mesh);
00424     tf::poseMsgToTF(im_to_eef, tf_im_to_eef);
00425     tf::Pose tf_eef_to_mesh = tf_root_to_link.inverse() * tf_root_to_mesh;
00426     tf::Pose tf_im_to_mesh = tf_im_to_eef * tf_eef_to_mesh;
00427     tf::Pose tf_root_to_mesh_new = tf_root_to_im * tf_im_to_mesh;
00428     tf::poseTFToMsg(tf_root_to_mesh_new, marker_array.markers[i].pose);
00429     // - - - - - - - - - - - - - - - - - - - - - - - - - -
00430     m_control.markers.push_back(marker_array.markers[i]);
00431   }
00432 
00433   im.controls.push_back(m_control);
00434 }
00435 
00436 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
00437                                         const EndEffectorInteraction& eef)
00438 {
00439   return "EE:" + handler->getName() + "_" + eef.parent_link;
00440 }
00441 
00442 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
00443                                         const JointInteraction& vj)
00444 {
00445   return "JJ:" + handler->getName() + "_" + vj.connecting_link;
00446 }
00447 
00448 static inline std::string getMarkerName(const ::robot_interaction::InteractionHandlerPtr& handler,
00449                                         const GenericInteraction& g)
00450 {
00451   return "GG:" + handler->getName() + "_" + g.marker_name_suffix;
00452 }
00453 
00454 void RobotInteraction::addInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00455                                              const double marker_scale)
00456 {
00457   handler->setRobotInteraction(this);
00458   // 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
00459   std::vector<visualization_msgs::InteractiveMarker> ims;
00460   ros::NodeHandle nh;
00461   {
00462     boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00463     robot_state::RobotStateConstPtr s = handler->getState();
00464 
00465     for (std::size_t i = 0; i < active_generic_.size(); ++i)
00466     {
00467       visualization_msgs::InteractiveMarker im;
00468       if (active_generic_[i].construct_marker(*s, im))
00469       {
00470         im.name = getMarkerName(handler, active_generic_[i]);
00471         shown_markers_[im.name] = i;
00472         ims.push_back(im);
00473         ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", im.name.c_str(),
00474                         im.scale);
00475       }
00476     }
00477     ros::NodeHandle nh;
00478 
00479     for (std::size_t i = 0; i < active_eef_.size(); ++i)
00480     {
00481       geometry_msgs::PoseStamped pose;
00482       geometry_msgs::Pose control_to_eef_tf;
00483       pose.header.frame_id = robot_model_->getModelFrame();
00484       pose.header.stamp = ros::Time::now();
00485       computeMarkerPose(handler, active_eef_[i], *s, pose.pose, control_to_eef_tf);
00486 
00487       std::string marker_name = getMarkerName(handler, active_eef_[i]);
00488       shown_markers_[marker_name] = i;
00489 
00490       // Determine interactive maker size
00491       double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_eef_[i].size : marker_scale;
00492 
00493       visualization_msgs::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
00494       if (handler && handler->getControlsVisible())
00495       {
00496         if (active_eef_[i].interaction & EEF_POSITION_ARROWS)
00497           addPositionControl(im, active_eef_[i].interaction & EEF_FIXED);
00498         if (active_eef_[i].interaction & EEF_ORIENTATION_CIRCLES)
00499           addOrientationControl(im, active_eef_[i].interaction & EEF_FIXED);
00500         if (active_eef_[i].interaction & (EEF_POSITION_SPHERE | EEF_ORIENTATION_SPHERE))
00501         {
00502           std_msgs::ColorRGBA color;
00503           color.r = 0;
00504           color.g = 1;
00505           color.b = 1;
00506           color.a = 0.5;
00507           addViewPlaneControl(im, mscale * 0.25, color, active_eef_[i].interaction & EEF_POSITION_SPHERE,
00508                               active_eef_[i].interaction & EEF_ORIENTATION_SPHERE);
00509         }
00510       }
00511       if (handler && handler->getMeshesVisible() &&
00512           (active_eef_[i].interaction & (EEF_POSITION_EEF | EEF_ORIENTATION_EEF)))
00513         addEndEffectorMarkers(handler, active_eef_[i], control_to_eef_tf, im,
00514                               active_eef_[i].interaction & EEF_POSITION_EEF,
00515                               active_eef_[i].interaction & EEF_ORIENTATION_EEF);
00516       ims.push_back(im);
00517       registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_eef_[i].parent_link);
00518       ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(),
00519                       mscale);
00520     }
00521     for (std::size_t i = 0; i < active_vj_.size(); ++i)
00522     {
00523       geometry_msgs::PoseStamped pose;
00524       pose.header.frame_id = robot_model_->getModelFrame();
00525       pose.header.stamp = ros::Time::now();
00526       tf::poseEigenToMsg(s->getGlobalLinkTransform(active_vj_[i].connecting_link), pose.pose);
00527       std::string marker_name = getMarkerName(handler, active_vj_[i]);
00528       shown_markers_[marker_name] = i;
00529 
00530       // Determine interactive maker size
00531       double mscale = marker_scale < std::numeric_limits<double>::epsilon() ? active_vj_[i].size : marker_scale;
00532 
00533       visualization_msgs::InteractiveMarker im = makeEmptyInteractiveMarker(marker_name, pose, mscale);
00534       if (handler && handler->getControlsVisible())
00535       {
00536         if (active_vj_[i].dof == 3)  // planar joint
00537           addPlanarXYControl(im, false);
00538         else
00539           add6DOFControl(im, false);
00540       }
00541       ims.push_back(im);
00542       registerMoveInteractiveMarkerTopic(marker_name, handler->getName() + "_" + active_vj_[i].connecting_link);
00543       ROS_DEBUG_NAMED("robot_interaction", "Publishing interactive marker %s (size = %lf)", marker_name.c_str(),
00544                       mscale);
00545     }
00546     handlers_[handler->getName()] = handler;
00547   }
00548 
00549   // we do this while marker_access_lock_ is unlocked because the interactive marker server locks
00550   // for most function calls, and maintains that lock while the feedback callback is running
00551   // that can cause a deadlock if we were to run the loop below while marker_access_lock_ is locked
00552   for (std::size_t i = 0; i < ims.size(); ++i)
00553   {
00554     int_marker_server_->insert(ims[i]);
00555     int_marker_server_->setCallback(ims[i].name,
00556                                     boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1));
00557 
00558     // Add menu handler to all markers that this interaction handler creates.
00559     if (boost::shared_ptr<interactive_markers::MenuHandler> mh = handler->getMenuHandler())
00560       mh->apply(*int_marker_server_, ims[i].name);
00561   }
00562 }
00563 
00564 void RobotInteraction::registerMoveInteractiveMarkerTopic(const std::string marker_name, const std::string& name)
00565 {
00566   ros::NodeHandle nh;
00567   std::stringstream ss;
00568   ss << "/rviz/moveit/move_marker/";
00569   ss << name;
00570   int_marker_move_topics_.push_back(ss.str());
00571   int_marker_names_.push_back(marker_name);
00572 }
00573 
00574 void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable)
00575 {
00576   if (enable)
00577   {
00578     boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00579     if (int_marker_move_subscribers_.size() == 0)
00580     {
00581       ros::NodeHandle nh;
00582       for (size_t i = 0; i < int_marker_move_topics_.size(); i++)
00583       {
00584         std::string topic_name = int_marker_move_topics_[i];
00585         std::string marker_name = int_marker_names_[i];
00586         int_marker_move_subscribers_.push_back(nh.subscribe<geometry_msgs::PoseStamped>(
00587             topic_name, 1, boost::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, _1)));
00588       }
00589     }
00590   }
00591   else
00592   {
00593     boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00594     int_marker_move_subscribers_.clear();
00595   }
00596 }
00597 
00598 void RobotInteraction::computeMarkerPose(const ::robot_interaction::InteractionHandlerPtr& handler,
00599                                          const EndEffectorInteraction& eef, const robot_state::RobotState& robot_state,
00600                                          geometry_msgs::Pose& pose, geometry_msgs::Pose& control_to_eef_tf) const
00601 {
00602   // Need to allow for control pose offsets
00603   tf::Transform tf_root_to_link, tf_root_to_control;
00604   tf::poseEigenToTF(robot_state.getGlobalLinkTransform(eef.parent_link), tf_root_to_link);
00605 
00606   geometry_msgs::Pose msg_link_to_control;
00607   if (handler->getPoseOffset(eef, msg_link_to_control))
00608   {
00609     tf::Transform tf_link_to_control;
00610     tf::poseMsgToTF(msg_link_to_control, tf_link_to_control);
00611 
00612     tf_root_to_control = tf_root_to_link * tf_link_to_control;
00613     tf::poseTFToMsg(tf_link_to_control.inverse(), control_to_eef_tf);
00614   }
00615   else
00616   {
00617     tf_root_to_control = tf_root_to_link;
00618     control_to_eef_tf.orientation.x = 0.0;
00619     control_to_eef_tf.orientation.y = 0.0;
00620     control_to_eef_tf.orientation.z = 0.0;
00621     control_to_eef_tf.orientation.w = 1.0;
00622   }
00623 
00624   tf::poseTFToMsg(tf_root_to_control, pose);
00625 }
00626 
00627 void RobotInteraction::updateInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler)
00628 {
00629   handler->setRobotInteraction(this);
00630   std::string root_link;
00631   std::map<std::string, geometry_msgs::Pose> pose_updates;
00632   {
00633     boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00634 
00635     robot_state::RobotStateConstPtr s = handler->getState();
00636     root_link = s->getRobotModel()->getModelFrame();
00637 
00638     for (std::size_t i = 0; i < active_eef_.size(); ++i)
00639     {
00640       std::string marker_name = getMarkerName(handler, active_eef_[i]);
00641       geometry_msgs::Pose control_to_eef_tf;
00642       computeMarkerPose(handler, active_eef_[i], *s, pose_updates[marker_name], control_to_eef_tf);
00643     }
00644 
00645     for (std::size_t i = 0; i < active_vj_.size(); ++i)
00646     {
00647       std::string marker_name = getMarkerName(handler, active_vj_[i]);
00648       tf::poseEigenToMsg(s->getGlobalLinkTransform(active_vj_[i].connecting_link), pose_updates[marker_name]);
00649     }
00650 
00651     for (std::size_t i = 0; i < active_generic_.size(); ++i)
00652     {
00653       std::string marker_name = getMarkerName(handler, active_generic_[i]);
00654       geometry_msgs::Pose pose;
00655       if (active_generic_[i].update_pose && active_generic_[i].update_pose(*s, pose))
00656         pose_updates[marker_name] = pose;
00657     }
00658   }
00659 
00660   std_msgs::Header header;
00661   header.frame_id = root_link;  // marker poses are give w.r.t. root frame
00662   for (std::map<std::string, geometry_msgs::Pose>::const_iterator it = pose_updates.begin(); it != pose_updates.end();
00663        ++it)
00664     int_marker_server_->setPose(it->first, it->second, header);
00665   int_marker_server_->applyChanges();
00666 }
00667 
00668 void RobotInteraction::publishInteractiveMarkers()
00669 {
00670   // the server locks internally, so we need not worry about locking
00671   int_marker_server_->applyChanges();
00672 }
00673 
00674 bool RobotInteraction::showingMarkers(const ::robot_interaction::InteractionHandlerPtr& handler)
00675 {
00676   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00677 
00678   for (std::size_t i = 0; i < active_eef_.size(); ++i)
00679     if (shown_markers_.find(getMarkerName(handler, active_eef_[i])) == shown_markers_.end())
00680       return false;
00681   for (std::size_t i = 0; i < active_vj_.size(); ++i)
00682     if (shown_markers_.find(getMarkerName(handler, active_vj_[i])) == shown_markers_.end())
00683       return false;
00684   for (std::size_t i = 0; i < active_generic_.size(); ++i)
00685     if (shown_markers_.find(getMarkerName(handler, active_generic_[i])) == shown_markers_.end())
00686       return false;
00687   return true;
00688 }
00689 
00690 // TODO: can we get rid of this?  Only used in moveit_ros/benchmarks_gui/src/tab_states_and_goals.cpp right now.
00691 bool RobotInteraction::updateState(robot_state::RobotState& state, const EndEffectorInteraction& eef,
00692                                    const geometry_msgs::Pose& pose, unsigned int attempts, double ik_timeout,
00693                                    const robot_state::GroupStateValidityCallbackFn& validity_callback,
00694                                    const kinematics::KinematicsQueryOptions& kinematics_query_options)
00695 {
00696   if (state.setFromIK(state.getJointModelGroup(eef.parent_group), pose, eef.parent_link,
00697                       kinematics_query_options.lock_redundant_joints ? 1 : attempts, ik_timeout, validity_callback,
00698                       kinematics_query_options))
00699   {
00700     state.update();
00701     return true;
00702   }
00703   return false;
00704 }
00705 
00706 void RobotInteraction::moveInteractiveMarker(const std::string name, const geometry_msgs::PoseStampedConstPtr& msg)
00707 {
00708   std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(name);
00709   if (it != shown_markers_.end())
00710   {
00711     visualization_msgs::InteractiveMarkerFeedback::Ptr feedback(new visualization_msgs::InteractiveMarkerFeedback);
00712     feedback->header = msg->header;
00713     feedback->marker_name = name;
00714     feedback->pose = msg->pose;
00715     feedback->event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
00716     processInteractiveMarkerFeedback(feedback);
00717     {
00718       boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00719       int_marker_server_->setPose(name, msg->pose, msg->header);  // move the interactive marker
00720       int_marker_server_->applyChanges();
00721     }
00722   }
00723 }
00724 
00725 void RobotInteraction::processInteractiveMarkerFeedback(
00726     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00727 {
00728   // perform some validity checks
00729   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00730   std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
00731   if (it == shown_markers_.end())
00732   {
00733     ROS_ERROR("Unknown marker name: '%s' (not published by RobotInteraction class)", feedback->marker_name.c_str());
00734     return;
00735   }
00736 
00737   std::size_t u = feedback->marker_name.find_first_of("_");
00738   if (u == std::string::npos || u < 4)
00739   {
00740     ROS_ERROR("Invalid marker name: '%s'", feedback->marker_name.c_str());
00741     return;
00742   }
00743 
00744   feedback_map_[feedback->marker_name] = feedback;
00745   new_feedback_condition_.notify_all();
00746 }
00747 
00748 void RobotInteraction::processingThread()
00749 {
00750   boost::unique_lock<boost::mutex> ulock(marker_access_lock_);
00751 
00752   while (run_processing_thread_ && ros::ok())
00753   {
00754     while (feedback_map_.empty() && run_processing_thread_ && ros::ok())
00755       new_feedback_condition_.wait(ulock);
00756 
00757     while (!feedback_map_.empty() && ros::ok())
00758     {
00759       visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback = feedback_map_.begin()->second;
00760       feedback_map_.erase(feedback_map_.begin());
00761       ROS_DEBUG_NAMED("robot_interaction", "Processing feedback from map for marker [%s]",
00762                       feedback->marker_name.c_str());
00763 
00764       std::map<std::string, std::size_t>::const_iterator it = shown_markers_.find(feedback->marker_name);
00765       if (it == shown_markers_.end())
00766       {
00767         ROS_ERROR("Unknown marker name: '%s' (not published by RobotInteraction class) "
00768                   "(should never have ended up in the feedback_map!)",
00769                   feedback->marker_name.c_str());
00770         continue;
00771       }
00772       std::size_t u = feedback->marker_name.find_first_of("_");
00773       if (u == std::string::npos || u < 4)
00774       {
00775         ROS_ERROR("Invalid marker name: '%s' (should never have ended up in the feedback_map!)",
00776                   feedback->marker_name.c_str());
00777         continue;
00778       }
00779       std::string marker_class = feedback->marker_name.substr(0, 2);
00780       std::string handler_name = feedback->marker_name.substr(3, u - 3);  // skip the ":"
00781       std::map<std::string, ::robot_interaction::InteractionHandlerPtr>::const_iterator jt =
00782           handlers_.find(handler_name);
00783       if (jt == handlers_.end())
00784       {
00785         ROS_ERROR("Interactive Marker Handler '%s' is not known.", handler_name.c_str());
00786         continue;
00787       }
00788 
00789       // we put this in a try-catch because user specified callbacks may be triggered
00790       try
00791       {
00792         if (marker_class == "EE")
00793         {
00794           // make a copy of the data, so we do not lose it while we are unlocked
00795           EndEffectorInteraction eef = active_eef_[it->second];
00796           ::robot_interaction::InteractionHandlerPtr ih = jt->second;
00797           marker_access_lock_.unlock();
00798           try
00799           {
00800             ih->handleEndEffector(eef, feedback);
00801           }
00802           catch (std::runtime_error& ex)
00803           {
00804             ROS_ERROR("Exception caught while handling end-effector update: %s", ex.what());
00805           }
00806           catch (...)
00807           {
00808             ROS_ERROR("Exception caught while handling end-effector update");
00809           }
00810           marker_access_lock_.lock();
00811         }
00812         else if (marker_class == "JJ")
00813         {
00814           // make a copy of the data, so we do not lose it while we are unlocked
00815           JointInteraction vj = active_vj_[it->second];
00816           ::robot_interaction::InteractionHandlerPtr ih = jt->second;
00817           marker_access_lock_.unlock();
00818           try
00819           {
00820             ih->handleJoint(vj, feedback);
00821           }
00822           catch (std::runtime_error& ex)
00823           {
00824             ROS_ERROR("Exception caught while handling joint update: %s", ex.what());
00825           }
00826           catch (...)
00827           {
00828             ROS_ERROR("Exception caught while handling joint update");
00829           }
00830           marker_access_lock_.lock();
00831         }
00832         else if (marker_class == "GG")
00833         {
00834           ::robot_interaction::InteractionHandlerPtr ih = jt->second;
00835           GenericInteraction g = active_generic_[it->second];
00836           marker_access_lock_.unlock();
00837           try
00838           {
00839             ih->handleGeneric(g, feedback);
00840           }
00841           catch (std::runtime_error& ex)
00842           {
00843             ROS_ERROR("Exception caught while handling joint update: %s", ex.what());
00844           }
00845           catch (...)
00846           {
00847             ROS_ERROR("Exception caught while handling joint update");
00848           }
00849           marker_access_lock_.lock();
00850         }
00851         else
00852           ROS_ERROR("Unknown marker class ('%s') for marker '%s'", marker_class.c_str(), feedback->marker_name.c_str());
00853       }
00854       catch (std::runtime_error& ex)
00855       {
00856         ROS_ERROR("Exception caught while processing event: %s", ex.what());
00857       }
00858       catch (...)
00859       {
00860         ROS_ERROR("Exception caught while processing event");
00861       }
00862     }
00863   }
00864 }
00865 
00866 // DEPRECATED FUNCTIONALITY for backwards compatibility
00867 void RobotInteraction::decideActiveComponents(const std::string& group, EndEffectorInteractionStyle style)
00868 {
00869   decideActiveComponents(group, (InteractionStyle::InteractionStyle)(int)style);
00870 }
00871 
00872 // DEPRECATED FUNCTIONALITY for backwards compatibility
00873 void RobotInteraction::decideActiveEndEffectors(const std::string& group, EndEffectorInteractionStyle style)
00874 {
00875   decideActiveEndEffectors(group, (InteractionStyle::InteractionStyle)(int)style);
00876 }
00877 }


robot_interaction
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:39