interaction_handler.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_INTERACTION_HANDLER_
00038 #define MOVEIT_ROBOT_INTERACTION_INTERACTION_HANDLER_
00039 
00040 #include <moveit/macros/class_forward.h>
00041 #include <moveit/robot_interaction/locked_robot_state.h>
00042 //#include <moveit/robot_interaction/robot_interaction.h>
00043 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00044 #include <interactive_markers/menu_handler.h>
00045 #include <tf/tf.h>
00046 
00047 namespace robot_interaction
00048 {
00049 MOVEIT_CLASS_FORWARD(InteractionHandler);
00050 MOVEIT_CLASS_FORWARD(RobotInteraction);
00051 MOVEIT_CLASS_FORWARD(KinematicOptionsMap);
00052 
00053 class EndEffectorInteraction;
00054 class JointInteraction;
00055 class GenericInteraction;
00056 
00066 typedef boost::function<void(InteractionHandler*, bool)> InteractionHandlerCallbackFn;
00067 
00076 class InteractionHandler : public LockedRobotState
00077 {
00078 public:
00079   // Use this constructor if you have an initial RobotState already.
00080   InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
00081                      const robot_state::RobotState& initial_robot_state,
00082                      const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>());
00083 
00084   // Use this constructor to start with a default state.
00085   InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
00086                      const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>());
00087 
00088   // DEPRECATED.
00089   InteractionHandler(const std::string& name, const robot_state::RobotState& initial_robot_state,
00090                      const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>());
00091   // DEPRECATED.
00092   InteractionHandler(const std::string& name, const robot_model::RobotModelConstPtr& model,
00093                      const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>());
00094 
00095   virtual ~InteractionHandler()
00096   {
00097   }
00098 
00099   const std::string& getName() const
00100   {
00101     return name_;
00102   }
00103 
00105   using LockedRobotState::getState;
00106 
00108   using LockedRobotState::setState;
00109 
00110   void setUpdateCallback(const InteractionHandlerCallbackFn& callback);
00111   const InteractionHandlerCallbackFn& getUpdateCallback() const;
00112   void setMeshesVisible(bool visible);
00113   bool getMeshesVisible() const;
00114   void setControlsVisible(bool visible);
00115   bool getControlsVisible() const;
00116 
00120   void setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::Pose& m);
00121 
00125   void setPoseOffset(const JointInteraction& j, const geometry_msgs::Pose& m);
00126 
00131   bool getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::Pose& m);
00132 
00137   bool getPoseOffset(const JointInteraction& vj, geometry_msgs::Pose& m);
00138 
00142   void clearPoseOffset(const EndEffectorInteraction& eef);
00143 
00146   void clearPoseOffset(const JointInteraction& vj);
00147 
00149   void clearPoseOffsets();
00150 
00154   void setMenuHandler(const boost::shared_ptr<interactive_markers::MenuHandler>& mh);
00155 
00159   const boost::shared_ptr<interactive_markers::MenuHandler>& getMenuHandler();
00160 
00162   void clearMenuHandler();
00163 
00169   bool getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef, geometry_msgs::PoseStamped& pose);
00170 
00176   bool getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::PoseStamped& pose);
00177 
00181   void clearLastEndEffectorMarkerPose(const EndEffectorInteraction& eef);
00182 
00185   void clearLastJointMarkerPose(const JointInteraction& vj);
00186 
00189   void clearLastMarkerPoses();
00190 
00193   virtual void handleEndEffector(const EndEffectorInteraction& eef,
00194                                  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00195 
00198   virtual void handleJoint(const JointInteraction& vj,
00199                            const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00200 
00203   virtual void handleGeneric(const GenericInteraction& g,
00204                              const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00205 
00208   virtual bool inError(const EndEffectorInteraction& eef) const;
00209 
00212   virtual bool inError(const JointInteraction& vj) const;
00213 
00215   virtual bool inError(const GenericInteraction& g) const;
00216 
00219   void clearError(void);
00220 
00224   void setRobotInteraction(RobotInteraction* robot_interaction);
00225 
00226 protected:
00227   bool transformFeedbackPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
00228                              const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose);
00229 
00230   const std::string name_;
00231   const std::string planning_frame_;
00232   boost::shared_ptr<tf::Transformer> tf_;
00233 
00234 private:
00235   typedef boost::function<void(InteractionHandler*)> StateChangeCallbackFn;
00236 
00237   // Update RobotState using a generic interaction feedback message.
00238   // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
00239   void updateStateGeneric(robot_state::RobotState* state, const GenericInteraction* g,
00240                           const visualization_msgs::InteractiveMarkerFeedbackConstPtr* feedback,
00241                           StateChangeCallbackFn* callback);
00242 
00243   // Update RobotState for a new pose of an eef.
00244   // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
00245   void updateStateEndEffector(robot_state::RobotState* state, const EndEffectorInteraction* eef,
00246                               const geometry_msgs::Pose* pose, StateChangeCallbackFn* callback);
00247 
00248   // Update RobotState for a new joint position.
00249   // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
00250   void updateStateJoint(robot_state::RobotState* state, const JointInteraction* vj, const geometry_msgs::Pose* pose,
00251                         StateChangeCallbackFn* callback);
00252 
00253   // Set the error state for \e name.
00254   // Returns true if the error state for \e name changed.
00255   // YOU MUST LOCK state_lock_ BEFORE CALLING THIS.
00256   bool setErrorState(const std::string& name, bool new_error_state);
00257 
00260   bool getErrorState(const std::string& name) const;
00261 
00262   // Contains the (user-programmable) pose offset between the end-effector
00263   // parent link (or a virtual joint) and the desired control frame for the
00264   // interactive marker. The offset is expressed in the frame of the parent
00265   // link or virtual joint. For example, on a PR2 an offset of +0.20 along
00266   // the x-axis will move the center of the 6-DOF interactive marker from
00267   // the wrist to the finger tips.
00268   // PROTECTED BY offset_map_lock_
00269   std::map<std::string, geometry_msgs::Pose> offset_map_;
00270 
00271   // Contains the most recent poses received from interactive marker feedback,
00272   // with the offset removed (e.g. in theory, coinciding with the end-effector
00273   // parent or virtual joint). This allows a user application to query for the
00274   // interactive marker pose (which could be useful for robot control using
00275   // gradient-based methods) even when the IK solver failed to find a valid
00276   // robot state that satisfies the feedback pose.
00277   // PROTECTED BY pose_map_lock_
00278   std::map<std::string, geometry_msgs::PoseStamped> pose_map_;
00279 
00280   // The RobotInteraction we are associated with.
00281   // This is never safe to use because the RobotInteraction could be deleted at
00282   // any time.
00283   // Therefore it is stored as a void* to discourage its use.
00284   // This is only used inside setKinematicOptions() with the state_lock_ held.
00285   // That function should only be called from RobotInteraction methods.
00286   // PROTECTED BY state_lock_
00287   const void* robot_interaction_;
00288 
00289   boost::mutex pose_map_lock_;
00290   boost::mutex offset_map_lock_;
00291 
00292   // per group options for doing kinematics.
00293   // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_.  The
00294   // CONTENTS is protected internally.
00295   KinematicOptionsMapPtr kinematic_options_map_;
00296 
00297   // A set of Interactions for which the pose is invalid.
00298   // PROTECTED BY state_lock_
00299   std::set<std::string> error_state_;
00300 
00301   // For adding menus (and associated callbacks) to all the
00302   // end-effector and virtual-joint interactive markers
00303   //
00304   // PROTECTED BY state_lock_ - The POINTER is protected by state_lock_.  The
00305   // CONTENTS is not.
00306   boost::shared_ptr<interactive_markers::MenuHandler> menu_handler_;
00307 
00308   // Called when the RobotState maintained by the handler changes.
00309   // The caller may, for example, redraw the robot at the new state.
00310   // handler is the handler that changed.
00311   // error_state_changed is true if an end effector's error state may have
00312   // changed.
00313   //
00314   // PROTECTED BY state_lock_ - the function pointer is protected, but the call
00315   // is made without any lock held.
00316   boost::function<void(InteractionHandler* handler, bool error_state_changed)> update_callback_;
00317 
00318   // PROTECTED BY state_lock_
00319   bool display_meshes_;
00320 
00321   // PROTECTED BY state_lock_
00322   bool display_controls_;
00323 
00324   // remove '_' characters from name
00325   static std::string fixName(std::string name);
00326 
00327 public:
00328   // DEPRECATED FUNCTIONS.
00329   // DO NOT USE THESE.  Instead access the KinematicOptions by calling
00330   // RobotInteraction::getKinematicOptionsMap()
00331   void setGroupStateValidityCallback(const robot_state::GroupStateValidityCallbackFn& callback);
00332   void setIKTimeout(double timeout);
00333   void setIKAttempts(unsigned int attempts);
00334   const kinematics::KinematicsQueryOptions& getKinematicsQueryOptions() const;
00335   void setKinematicsQueryOptions(const kinematics::KinematicsQueryOptions& opt);
00336   void setKinematicsQueryOptionsForGroup(const std::string& group_name,
00337                                          const kinematics::KinematicsQueryOptions& options);
00338 };
00339 }
00340 
00341 #endif


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