robot_interaction.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Adam Leeper */
00036 
00037 #ifndef MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_
00038 #define MOVEIT_ROBOT_INTERACTION_ROBOT_INTERACTION_
00039 
00040 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00041 #include <visualization_msgs/InteractiveMarker.h>
00042 #include <geometry_msgs/PoseArray.h>
00043 #include <interactive_markers/menu_handler.h>
00044 #include <moveit/macros/class_forward.h>
00045 #include <moveit/robot_state/robot_state.h>
00046 #include <moveit/robot_interaction/interaction.h>
00047 #include <boost/function.hpp>
00048 #include <boost/thread.hpp>
00049 
00050 // This is needed for legacy code that includes robot_interaction.h but not
00051 // interaction_handler.h
00052 #include <moveit/robot_interaction/interaction_handler.h>
00053 
00054 namespace interactive_markers
00055 {
00056 class InteractiveMarkerServer;
00057 }
00058 
00059 namespace robot_interaction
00060 {
00061 MOVEIT_CLASS_FORWARD(InteractionHandler);
00062 MOVEIT_CLASS_FORWARD(KinematicOptionsMap);
00063 MOVEIT_CLASS_FORWARD(RobotInteraction);
00064 
00065 // Manage interactive markers for controlling a robot state.
00066 //
00067 // The RobotInteraction class manages one or more InteractionHandler objects
00068 // each of which maintains a set of interactive markers for manipulating one
00069 // group of one RobotState.
00070 //
00071 // The group being manipulated is common to all InteractionHandler objects
00072 // contained in a RobotInteraction instance.
00073 class RobotInteraction
00074 {
00075 public:
00076   // DEPRECATED TYPES.  For backwards compatibility.  Avoid using these.
00077   typedef ::robot_interaction::InteractionHandler InteractionHandler;
00078   typedef ::robot_interaction::InteractionHandlerPtr InteractionHandlerPtr;
00079   typedef ::robot_interaction::EndEffectorInteraction EndEffector;
00080   typedef ::robot_interaction::JointInteraction Joint;
00081   typedef ::robot_interaction::GenericInteraction Generic;
00082 
00083 public:
00085   static const std::string INTERACTIVE_MARKER_TOPIC;
00086 
00087   RobotInteraction(const robot_model::RobotModelConstPtr& kmodel, const std::string& ns = "");
00088   virtual ~RobotInteraction();
00089 
00090   const std::string& getServerTopic(void) const
00091   {
00092     return topic_;
00093   }
00094 
00106   void addActiveComponent(const InteractiveMarkerConstructorFn& construct, const ProcessFeedbackFn& process,
00107                           const InteractiveMarkerUpdateFn& update = InteractiveMarkerUpdateFn(),
00108                           const std::string& name = "");
00109 
00118   void decideActiveComponents(const std::string& group);
00119   void decideActiveComponents(const std::string& group, InteractionStyle::InteractionStyle style);
00120 
00123   void clear();
00124 
00130   void addInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00131                              const double marker_scale = 0.0);
00132 
00133   // Update pose of all interactive markers to match the handler's RobotState.
00134   // Call this when the handler's RobotState changes.
00135   void updateInteractiveMarkers(const ::robot_interaction::InteractionHandlerPtr& handler);
00136 
00137   // True if markers are being shown for this handler.
00138   bool showingMarkers(const ::robot_interaction::InteractionHandlerPtr& handler);
00139 
00140   // Display all markers that have been added.
00141   // This is needed after calls to addInteractiveMarkers() to publish the
00142   // resulting markers so they get displayed.  This call is not needed after
00143   // calling updateInteractiveMarkers() which publishes the results itself.
00144   void publishInteractiveMarkers();
00145 
00146   // Clear all interactive markers.
00147   // This removes all interactive markers but does not affect which
00148   // interactions are active.  After this a call to publishInteractiveMarkers()
00149   // is needed to actually remove the markers from the display.
00150   void clearInteractiveMarkers();
00151 
00152   const robot_model::RobotModelConstPtr& getRobotModel() const
00153   {
00154     return robot_model_;
00155   }
00156 
00157   // Get the kinematic options map.
00158   // Use this to set kinematic options (defaults or per-group).
00159   KinematicOptionsMapPtr getKinematicOptionsMap()
00160   {
00161     return kinematic_options_map_;
00162   }
00163 
00164   // enable/disable subscription of the topics to move interactive marker
00165   void toggleMoveInteractiveMarkerTopic(bool enable);
00166 
00167 private:
00168   // called by decideActiveComponents(); add markers for end effectors
00169   void decideActiveEndEffectors(const std::string& group);
00170   void decideActiveEndEffectors(const std::string& group, InteractionStyle::InteractionStyle style);
00171 
00172   // called by decideActiveComponents(); add markers for planar and floating
00173   // joints
00174   void decideActiveJoints(const std::string& group);
00175 
00176   void moveInteractiveMarker(const std::string name, const geometry_msgs::PoseStampedConstPtr& msg);
00177   // register the name of the topic and marker name to move
00178   // interactive marker from other ROS nodes
00179   void registerMoveInteractiveMarkerTopic(const std::string marker_name, const std::string& name);
00180   // return the diameter of the sphere that certainly can enclose the AABB of the link
00181   double computeLinkMarkerSize(const std::string& link);
00182   // return the diameter of the sphere that certainly can enclose the AABB of
00183   // the links in this group
00184   double computeGroupMarkerSize(const std::string& group);
00185   void computeMarkerPose(const ::robot_interaction::InteractionHandlerPtr& handler, const EndEffectorInteraction& eef,
00186                          const robot_state::RobotState& robot_state, geometry_msgs::Pose& pose,
00187                          geometry_msgs::Pose& control_to_eef_tf) const;
00188 
00189   void addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00190                              const EndEffectorInteraction& eef, visualization_msgs::InteractiveMarker& im,
00191                              bool position = true, bool orientation = true);
00192   void addEndEffectorMarkers(const ::robot_interaction::InteractionHandlerPtr& handler,
00193                              const EndEffectorInteraction& eef, const geometry_msgs::Pose& offset,
00194                              visualization_msgs::InteractiveMarker& im, bool position = true, bool orientation = true);
00195   void processInteractiveMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00196   void subscribeMoveInteractiveMarker(const std::string marker_name, const std::string& name);
00197   void processingThread();
00198   void clearInteractiveMarkersUnsafe();
00199 
00200   boost::scoped_ptr<boost::thread> processing_thread_;
00201   bool run_processing_thread_;
00202 
00203   boost::condition_variable new_feedback_condition_;
00204   std::map<std::string, visualization_msgs::InteractiveMarkerFeedbackConstPtr> feedback_map_;
00205 
00206   robot_model::RobotModelConstPtr robot_model_;
00207 
00208   std::vector<EndEffectorInteraction> active_eef_;
00209   std::vector<JointInteraction> active_vj_;
00210   std::vector<GenericInteraction> active_generic_;
00211 
00212   std::map<std::string, ::robot_interaction::InteractionHandlerPtr> handlers_;
00213   std::map<std::string, std::size_t> shown_markers_;
00214 
00215   // This mutex is locked every time markers are read or updated;
00216   // This includes the active_* arrays and shown_markers_
00217   // Please note that this mutex *MUST NOT* be locked while operations
00218   // on the interative marker server are called because the server
00219   // also locks internally and we could othewrise end up with a problem
00220   // of Thread 1: Lock A,         Lock B, Unlock B, Unloack A
00221   //    Thread 2:         Lock B, Lock A
00222   // => deadlock
00223   boost::mutex marker_access_lock_;
00224 
00225   interactive_markers::InteractiveMarkerServer* int_marker_server_;
00226   // ros subscribers to move the interactive markers by other ros nodes
00227   std::vector<ros::Subscriber> int_marker_move_subscribers_;
00228   // the array of the names of the topics which need to be subscribed
00229   // to move the interactive markers by other ROS nodes
00230   std::vector<std::string> int_marker_move_topics_;
00231   // the array of the marker names in the same order to int_marker_move_topics_
00232   std::vector<std::string> int_marker_names_;
00233 
00234   std::string topic_;
00235 
00236   // options for doing IK
00237   // Locking is done internally
00238   KinematicOptionsMapPtr kinematic_options_map_;
00239 
00240 public:
00241   // DEPRECATED.  This is included for backwards compatibility.
00242   // These classes/enums used to be subclasses of RobotInteraction.  This
00243   // allows client code to continue to work as if they are.
00244 
00247   enum EndEffectorInteractionStyle
00248   {
00249     EEF_POSITION_ARROWS = InteractionStyle::POSITION_ARROWS,
00250     EEF_ORIENTATION_CIRCLES = InteractionStyle::ORIENTATION_CIRCLES,
00251     EEF_POSITION_SPHERE = InteractionStyle::POSITION_SPHERE,
00252     EEF_ORIENTATION_SPHERE = InteractionStyle::ORIENTATION_SPHERE,
00253     EEF_POSITION_EEF = InteractionStyle::POSITION_EEF,
00254     EEF_ORIENTATION_EEF = InteractionStyle::ORIENTATION_EEF,
00255     EEF_FIXED = InteractionStyle::FIXED,
00256     EEF_POSITION = InteractionStyle::POSITION,
00257     EEF_ORIENTATION = InteractionStyle::ORIENTATION,
00258     EEF_6DOF = InteractionStyle::SIX_DOF,
00259     EEF_6DOF_SPHERE = InteractionStyle::SIX_DOF_SPHERE,
00260     EEF_POSITION_NOSPHERE = InteractionStyle::POSITION_NOSPHERE,
00261     EEF_ORIENTATION_NOSPHERE = InteractionStyle::ORIENTATION_NOSPHERE,
00262     EEF_6DOF_NOSPHERE = InteractionStyle::SIX_DOF_NOSPHERE
00263   };
00264   // DEPRECATED.  Use InteractionStyle::InteractionStyle version.
00265   void decideActiveComponents(const std::string& group, EndEffectorInteractionStyle style);
00266   // DEPRECATED.  Use InteractionStyle::InteractionStyle version.
00267   void decideActiveEndEffectors(const std::string& group, EndEffectorInteractionStyle style);
00268   // DEPRECATED
00269   const std::vector<EndEffectorInteraction>& getActiveEndEffectors() const
00270   {
00271     return active_eef_;
00272   }
00273   // DEPRECATED
00274   const std::vector<JointInteraction>& getActiveJoints() const
00275   {
00276     return active_vj_;
00277   }
00278   // DEPRECATED
00279   static bool updateState(
00280       robot_state::RobotState& state, const EndEffectorInteraction& eef, const geometry_msgs::Pose& pose,
00281       unsigned int attempts, double ik_timeout,
00282       const robot_state::GroupStateValidityCallbackFn& validity_callback = robot_state::GroupStateValidityCallbackFn(),
00283       const kinematics::KinematicsQueryOptions& kinematics_query_options = kinematics::KinematicsQueryOptions());
00284 };
00285 }
00286 
00287 #endif


robot_interaction
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:55